diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 46cae7e..4d71821 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -22,7 +22,7 @@ #define sv_extr extern #endif -#define dim 20 +#define dim 50 #define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)] ) #define CURRENT_DATA_WRITE ( datas[((currentSVIndex))] ) #define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] ) @@ -60,4 +60,6 @@ sv_extr data datas[dim]; sv_extr int currentSVIndex; void initStatusVector(); -void updateStatusVector(); \ No newline at end of file +void updateStatusVector(); +data getDataAtIndex(int index); +data getDataAtIndex_backwardsFromCurrent(int steps); \ No newline at end of file diff --git a/include/motors_movement/roller.h b/include/motors_movement/roller.h index d7242b5..f6c717f 100644 --- a/include/motors_movement/roller.h +++ b/include/motors_movement/roller.h @@ -16,5 +16,5 @@ class Roller{ int pinPwm, pinSense, MIN, MAX, ARM; bool roller_arm_setup, roller_armed; - int roller_setup_phase, roller_counter; + int roller_setup_phase, roller_counter, roller_speed; }; \ No newline at end of file diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index b128b26..f679a82 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -17,8 +17,7 @@ #define S4O A8 #define LINE_THRESH_CAM 325 -#define EXIT_TIME 250 -#define LINES_EXIT_SPD 350 +#define EXIT_TIME 300 class LineSysCamera : public LineSystem{ diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 018cb50..069aa30 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -3,6 +3,7 @@ #include "PID_v2.h" #include "systems/systems.h" #include "behaviour_control/complementary_filter.h" +#include "behaviour_control/status_vector.h" /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. @@ -55,4 +56,6 @@ class PositionSysCamera : public PositionSystem{ ComplementaryFilter* filterDir; ComplementaryFilter* filterSpeed; + data valid_data; + }; diff --git a/src/behaviour_control/status_vector.cpp b/src/behaviour_control/status_vector.cpp index 1fd9ca0..e9be4ef 100644 --- a/src/behaviour_control/status_vector.cpp +++ b/src/behaviour_control/status_vector.cpp @@ -10,8 +10,19 @@ void initStatusVector(){ } } +data getDataAtIndex(int index){ + index = constrain(index, 0, dim-1); + return datas[index]; +} + +data getDataAtIndex_backwardsFromCurrent(int steps){ + steps = constrain(steps, 0, dim-1); + return getDataAtIndex((currentSVIndex-steps+dim) % dim); +} + void updateStatusVector(){ currentSVIndex = (currentSVIndex+1) % dim; CURRENT_DATA_WRITE = CURRENT_DATA_READ; CURRENT_INPUT_WRITE = CURRENT_INPUT_READ; -} \ No newline at end of file +} + diff --git a/src/main.cpp b/src/main.cpp index 69b2007..1c98718 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,6 +17,7 @@ bool striker_condition = false; bool keeper_condition = false; void setup() { + tone(BUZZER, 220, 250); delay(1500); DEBUG.begin(115200); @@ -26,8 +27,11 @@ void setup() { } testmenu = new TestMenu(); + tone(BUZZER, 240, 250); initStatusVector(); + tone(BUZZER, 260, 250); initSensors(); + tone(BUZZER, 320, 250); initGames(); delay(500); @@ -35,7 +39,7 @@ void setup() { drive->prepareDrive(0,0,0); //Startup sound - tone(BUZZER, 220.00, 250); + tone(BUZZER, 350.00, 250); } void loop() { @@ -48,7 +52,7 @@ void loop() { striker->play(striker_condition); keeper->play(keeper_condition); - testmenu->testMenu(); + // testmenu->testMenu(); // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index f8eea5d..343464e 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -121,15 +121,15 @@ void DriveController::drive(int dir, int speed, int tilt){ speed3 /= ratio; speed4 /= ratio; - DEBUG.print(speed1); - DEBUG.print(" | "); - DEBUG.print(speed2); - DEBUG.print(" | "); - DEBUG.print(speed3); - DEBUG.print(" | "); - DEBUG.print(speed4); - DEBUG.print(" | "); - DEBUG.println(maxVel); + // DEBUG.print(speed1); + // DEBUG.print(" | "); + // DEBUG.print(speed2); + // DEBUG.print(" | "); + // DEBUG.print(speed3); + // DEBUG.print(" | "); + // DEBUG.print(speed4); + // DEBUG.print(" | "); + // DEBUG.println(maxVel); } speed1 = constrain(speed1, -255, 255); diff --git a/src/motors_movement/roller.cpp b/src/motors_movement/roller.cpp index c107f9f..00ee67e 100644 --- a/src/motors_movement/roller.cpp +++ b/src/motors_movement/roller.cpp @@ -22,7 +22,8 @@ Roller::Roller(int pinPwm_, int pinSense_, int MIN_, int MAX_, int ARM_){ roller_setup_phase = 99; roller_counter = 0; - + roller_speed = 0; + pinMode(pinSense, INPUT_PULLUP); roller = new ESC(pinPwm, MIN, MAX, ARM); } @@ -85,12 +86,12 @@ void Roller::update(){ this->setup(); if(millis() - t > 10 && roller_armed){ - roller->speed(1300); + roller->speed(roller_speed); t = millis(); } } -void Roller::speed(int speed){ +void Roller::speed(int speed_){ if(roller_armed) - roller->speed(speed); + roller_speed = speed_; } \ No newline at end of file diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index e2acb22..7c537c6 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -7,10 +7,18 @@ void initSensors(){ pinMode(SWITCH_ID, INPUT); drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305)); + // tone(BUZZER, 270, 250); + // delay(350); compass = new DataSourceBNO055(); + // tone(BUZZER, 275, 250); + // delay(350); ball = new DataSourceBall(&BALL_32U4, 57600); + // tone(BUZZER, 280, 250); + // delay(350); camera = new DataSourceCameraConic(&Serial3, 19200); - bt = new DataSourceBT(&Serial1, 115200); + // tone(BUZZER, 285, 250); + // delay(350); + // bt = new DataSourceBT(&Serial1, 115200); roller = new Roller(30, 31, 1000, 2000, 500); } diff --git a/src/system/lines/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp index a28bdbd..5e7bef2 100644 --- a/src/system/lines/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -78,7 +78,7 @@ void LineSysCamera::outOfBounds(){ } if (millis() - exitTimer < EXIT_TIME){ - CURRENT_DATA_WRITE.game->ps->goCenter(); + CURRENT_DATA_READ.game->ps->goCenter(); tookLine = true; tone(BUZZER, 220.00, 250); }else{ diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index ec8bb81..091406e 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -30,7 +30,7 @@ PositionSysCamera::PositionSysCamera() { Y->SetDerivativeLag(1); Y->SetSampleTime(2); - filterDir = new ComplementaryFilter(0.65); + filterDir = new ComplementaryFilter(0.35); filterSpeed = new ComplementaryFilter(0.65); } @@ -49,8 +49,24 @@ void PositionSysCamera::update(){ posx = CURRENT_DATA_WRITE.cam_xy; posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy); }else{ - //TODO: no goal seen ? + // Go back in time until we found a valid status, when we saw at least one goal + int i = 1; + do{ + valid_data = getDataAtIndex_backwardsFromCurrent(i); + i++; + }while(!valid_data.ySeen && !valid_data.bSeen); + + if(valid_data.ySeen || valid_data.bSeen){ + posx = valid_data.posx; + posy = valid_data.posy; + + // Trick the status vector into thinking this was a valid status + CURRENT_DATA_WRITE.ySeen = valid_data.ySeen; + CURRENT_DATA_WRITE.bSeen = valid_data.bSeen; + + } } + //IMPORTANT STEP: or the direction of the plane will be flipped posx *= -1; posy *= -1; @@ -123,9 +139,9 @@ void PositionSysCamera::CameraPID(){ int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) ); // int dist = sqrt(Outputx*Outputx + Outputy*Outputy); int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL); - speed = filterSpeed->calculate(speed); - speed = speed > 40 ? speed : 0; - dir = filterDir->calculate(dir); + speed = speed > 30 ? speed : 0; + dir = filterDir->calculate(dir);; + //speed = filterSpeed->calculate(speed); drive->prepareDrive(dir, speed, 0); diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 3e41553..bcb566b 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -60,6 +60,7 @@ void TestMenu::testMenu() drive->m2->test(); drive->m3->test(); drive->m4->test(); + flagtest = true; break; case '4': DEBUG.println("Pid recenter test"); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index ed3407d..536873c 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -46,7 +46,7 @@ blue_led.on() thresholds = [ (55, 92, -3, 24, 60, 90), # thresholds yellow goal - (33, 49, -9, 12, -52, -12)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (45, 61, -18, 12, -55, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (30, 0, 290, 240) @@ -94,7 +94,7 @@ while(True): tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata img = sensor.snapshot() - for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=90, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=40, area_threshold=50, merge = True): img.draw_rectangle(blob.rect()) #img.draw_cross(blob.cx(), blob.cy())