motors: complementary filter on a per-motor speed

avoid all the cons of bang-bang without spending 5mS ramping up and down

include/motors_movement/motor.h

@@ -12,9 +12,11 @@ class Motor {
         Motor();
         void drive(int speed);
         void test();
-    
-    public:
-        int pinA, pinB, pinPwm, angle, oldSpeed, diff;
-        float micros_wait;
 
+    public:
+        int angle;
+
+    private:
+        int pinA, pinB, pinPwm;
+        ComplementaryFilter* filter;
 };

include/strategy_roles/striker.h

@@ -4,7 +4,7 @@
 #include "sensors/sensors.h"
 #include "strategy_roles/game.h"
 
-#define STRIKER_ATTACK_DISTANCE 125
+#define STRIKER_ATTACK_DISTANCE 300
 #define STRIKER_PLUSANG 55
 #define STRIKER_PLUSANG_VISIONCONE 20
 

src/main.cpp

@@ -37,6 +37,8 @@ void loop() {
   updateSensors();
   if(DEBUG.available()) testmenu->testMenu();
   
+  // drive->prepareDrive(0,0,0);
+
   striker->play(1);
   // striker_test->play(1);
   // keeper->play(role==0);

src/motors_movement/motor.cpp

@@ -7,114 +7,32 @@ Motor::Motor(int a, int b, int pwm, int angle_){
     this->pinB = b;
     this->pinPwm = pwm;
     this->angle = angle_;
-    this->oldSpeed = 0;
+
-    this->diff = 0;
+    filter = new ComplementaryFilter(0.8);
-    this->micros_wait = 0;
 
     pinMode(pinA, OUTPUT);
     pinMode(pinB, OUTPUT);
     pinMode(pinPwm, OUTPUT);
-    analogWriteFrequency(pinPwm, 15000);
+    analogWriteFrequency(pinPwm, 5000);
 }
 
 Motor::Motor(){ }
 
 void Motor::drive(int speed){
+    speed = filter->calculate(speed);
 
-    // Create a smooth transitioning between the old and new speed value, to avoid the motor drivers going into overcurrent protection
+    if(speed > 0){
-    diff = abs(abs(speed) - abs(oldSpeed));
-
-    if(diff < 10) return;
-
-    micros_wait = (float)DRIVE_DURATION_MS/diff * 1000;
-    float micros_wait_half = micros_wait * 0.5;
-    
-    if(oldSpeed > 0 && speed > 0){
-        
         digitalWriteFast(pinA, HIGH);
         digitalWriteFast(pinB, LOW);
-
+    }else if(speed < 0){
-        if(oldSpeed < speed){
-            // Ramp up 
-            for(int i = oldSpeed; i < speed; i++){
-                analogWrite(pinPwm, i);
-            }
-            
-        } else if(oldSpeed > speed) {
-            // Slow down
-            for(int i = oldSpeed; i > speed; i--){
-                analogWrite(pinPwm, i);
-            }
-        }
-
-    }
-    else if(oldSpeed < 0 && speed < 0){
         digitalWriteFast(pinA, LOW);
         digitalWriteFast(pinB, HIGH);
-
+        speed *= -1;
-        if(oldSpeed < speed){
+    }else{
-            // Ramp up 
+        digitalWriteFast(pinA, LOW);
-            for(int i = oldSpeed; i < speed; i++){
+        digitalWriteFast(pinB, LOW);
-                analogWrite(pinPwm, -i);
-            }
-            
-        } else if(oldSpeed > speed) {
-            // Slow down
-            for(int i = oldSpeed; i > speed; i--){
-                analogWrite(pinPwm, -i);
-            }
-
-        }
-
     }
-    else if(oldSpeed < 0 && speed > 0){
+    analogWrite(pinPwm, speed);
-
-        digitalWriteFast(pinA, LOW);
-        digitalWriteFast(pinB, HIGH);
-
-        for(int i = oldSpeed; i <= 0; i++){
-            analogWrite(pinPwm, -i);
-        }
-
-        digitalWriteFast(pinA, LOW);
-        digitalWriteFast(pinB, LOW);
-
-        delayMicroseconds(micros_wait);
-
-        digitalWriteFast(pinA, HIGH);
-        digitalWriteFast(pinB, LOW);
-
-        for(int i = 0; i < speed; i++){
-            analogWrite(pinPwm, i);
-        }
-
-    }else if(oldSpeed > 0 && speed < 0){
-
-        digitalWriteFast(pinA, HIGH);
-        digitalWriteFast(pinB, LOW);
-
-        for(int i = oldSpeed; i >= 0; i--){
-            analogWrite(pinPwm, i);
-            delayMicroseconds(micros_wait_half);
-        }
-
-
-        digitalWriteFast(pinA, LOW);
-        digitalWriteFast(pinB, LOW);
-
-        delayMicroseconds(micros_wait);
-
-        digitalWriteFast(pinA, LOW);
-        digitalWriteFast(pinB, HIGH);
-
-        for(int i = 0; i > speed; i--){
-            analogWrite(pinPwm, -i);
-            delayMicroseconds(micros_wait_half);
-        }
-
-    }
-
-    oldSpeed = speed;
 }
 
 void Motor::test(){

src/system/positions/positionsys_camera.cpp

@@ -118,7 +118,7 @@ void PositionSysCamera::CameraPID(){
         dir = (dir+360) % 360;
 
         int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
-        int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 80);
+        int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
         drive->prepareDrive(dir, speed, 0);
 
 

utility/OpenMV/conic_eff_h7.py

@@ -45,8 +45,8 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (67, 100, -14, 28, 32, 58),    # thresholds yellow goal
+thresholds = [  (34, 64, -11, 7, 31, 71),    # thresholds yellow goal
-                (53, 69, -21, 11, -44, -13)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (24, 44, -25, 7, -36, -10)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (50,5,250, 230)
 
@@ -69,15 +69,15 @@ sensor.set_framesize(sensor.QVGA)
 sensor.set_windowing(roi)
 sensor.set_contrast(0)
 sensor.set_saturation(3)
-sensor.set_brightness(2)
+sensor.set_brightness(1)
 sensor.set_auto_whitebal(True)
-sensor.set_auto_exposure(False, 7500)
+sensor.set_auto_exposure(False, 2500)
-sensor.set_auto_gain(False, gain_db=20)
+sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
 clock = time.clock()
 ##############################################################################
-e
+
 
 while(True):
     clock.tick()