From 71cf8ff24944afac0afbee490156b16f6e1bd107 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 1 Mar 2021 16:28:01 +0100 Subject: [PATCH] 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 | 10 +- include/strategy_roles/striker.h | 2 +- src/main.cpp | 2 + src/motors_movement/motor.cpp | 104 +++----------------- src/system/positions/positionsys_camera.cpp | 2 +- utility/OpenMV/conic_eff_h7.py | 12 +-- 6 files changed, 27 insertions(+), 105 deletions(-) diff --git a/include/motors_movement/motor.h b/include/motors_movement/motor.h index 4c8dc7b..f402b4d 100644 --- a/include/motors_movement/motor.h +++ b/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; }; \ No newline at end of file diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index b552694..0f3b29b 100644 --- a/include/strategy_roles/striker.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 5d881d3..94c97fb 100755 --- a/src/main.cpp +++ b/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); diff --git a/src/motors_movement/motor.cpp b/src/motors_movement/motor.cpp index 4765329..0e5792f 100644 --- a/src/motors_movement/motor.cpp +++ b/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; - this->micros_wait = 0; + + filter = new ComplementaryFilter(0.8); 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 - 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){ - + if(speed > 0){ digitalWriteFast(pinA, HIGH); digitalWriteFast(pinB, LOW); - - 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){ + }else if(speed < 0){ digitalWriteFast(pinA, LOW); digitalWriteFast(pinB, HIGH); - - 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); - } - - } - + speed *= -1; + }else{ + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, LOW); } - else if(oldSpeed < 0 && speed > 0){ - - 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; + analogWrite(pinPwm, speed); } void Motor::test(){ diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index b2d90d3..253cb6a 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/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); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 48fe8cb..5a55a98 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,8 +45,8 @@ blue_led.on() ############################################################################## -thresholds = [ (67, 100, -14, 28, 32, 58), # thresholds yellow goal - (53, 69, -21, 11, -44, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (34, 64, -11, 7, 31, 71), # thresholds yellow goal + (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_gain(False, gain_db=20) +sensor.set_auto_exposure(False, 2500) +sensor.set_auto_gain(True) sensor.skip_frames(time = 300) clock = time.clock() ############################################################################## -e + while(True): clock.tick()