Speed can now also be negative

Author: MaartenS11

src/Primitives/Mindstorms/Motor.cpp

@@ -113,16 +113,19 @@ struct PID {
     }
 };
 
-void Motor::drive_to_target(int32_t max_speed) {
+void Motor::drive_to_target(int32_t _s) {
     printf("drift = %d\n", abs(get_drift()));
 
     // ev3 has rpm of 160-170
     // so maximum around a 1000 degrees per second
-    float target_speed = 250.0f; // Degrees per second
+    float max_speed = 250.0f; // Degrees per second
+    float target_speed = max_speed; // Degrees per second
 
     PID pid(1.0f, 0.05f, 0.2f);
+    PID position_pid(1.0f, 0.05f, 0.2f);
 
     while (true) {
+        // Control speed.
         float encoder_speed = encoder->get_speed();
         float error = target_speed - encoder_speed;
 
@@ -132,6 +135,11 @@ void Motor::drive_to_target(int32_t max_speed) {
         float normalized_speed = speed / 10000.0f;
         set_speed(normalized_speed);
 
+        // Control position.
+        /*error = static_cast<float>(get_drift());
+        output = position_pid.update(error);
+        target_speed = clamp(output, -max_speed, -max_speed);*/
+
         //printf("error = %f, speed = %f, integral = %f, derivative = %f, encoder_speed = %f\n", error, speed, integral, derivative, encoder_speed);
     }
     halt();

src/Primitives/Mindstorms/Motor.h

@@ -24,14 +24,18 @@ class MotorEncoder {
         if (rising) {
             if (pin5) {
                 encoder->angle++;
+                encoder->ticks++;
             } else {
                 encoder->angle--;
+                encoder->ticks--;
             }
         } else {
             if (pin5) {
                 encoder->angle--;
+                encoder->ticks--;
             } else {
                 encoder->angle++;
+                encoder->ticks++;
             }
         }
         int64_t t = k_uptime_get();
@@ -40,7 +44,7 @@ class MotorEncoder {
         //encoder->speed = 0.2f * encoder->speed + 0.8f * (1.0f / (t - encoder->last_update));
         //printf("measured speed after = %f\n", encoder->speed);
         encoder->last_update = t;
-        encoder->ticks++;
+        //encoder->ticks++;
     }
 
    public: