From 8634041ecfc2be08148e2b2687f5693abd4f0620 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 12 Feb 2020 21:43:20 +0100 Subject: [PATCH] status vector now working better. Still have to fix something --- .vscode/extensions.json | 14 +-- include/data_source_ball.h | 1 - include/drivecontroller.h | 2 +- include/status_vector.h | 28 +++-- src/data_source_ball.cpp | 16 +-- src/data_source_bno055.cpp | 6 +- src/data_source_camera_conicmirror.cpp | 14 ++- src/drivecontroller.cpp | 5 +- src/goalie.cpp | 18 ++-- src/main.cpp | 2 + src/status_vector.cpp | 2 +- utility/OpenMV/main_test_conic.py | 13 +-- utility/OpenMV/main_test_conic.py.autosave | 117 --------------------- 13 files changed, 70 insertions(+), 168 deletions(-) delete mode 100644 utility/OpenMV/main_test_conic.py.autosave diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 272828b..0f0d740 100755 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,7 +1,7 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ] -} \ No newline at end of file +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/include/data_source_ball.h b/include/data_source_ball.h index fdffeff..6beb7be 100755 --- a/include/data_source_ball.h +++ b/include/data_source_ball.h @@ -10,5 +10,4 @@ class DataSourceBall : public DataSource{ int angle, distance; bool ballSeen; - int dir, degrees2,b; }; \ No newline at end of file diff --git a/include/drivecontroller.h b/include/drivecontroller.h index d5d20af..5fcdc70 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -7,7 +7,7 @@ //PID Constants #define KP 1.2 #define KI 0.0 -#define KD 0.25 +#define KD 0.5 #define UNLOCK_THRESH 800 diff --git a/include/status_vector.h b/include/status_vector.h index cfaa29c..a9e18d1 100644 --- a/include/status_vector.h +++ b/include/status_vector.h @@ -1,7 +1,19 @@ #pragma once - #include +/** + * STATUS VECTOR: + * The status vector consists in two arrays of two different structs. + * One (inputs) holds the raw input read by the various sensors on the robot + * The other (datas) contains the useful data obtained by the eventual manipulation of the raw inputs + * This is made so that it ha an history of the inputs and datas if needed. + * This is an intermediator between all the classes representing the different components of the robot. It's preferable to not make the classes call one another + * All the data held by the structs in the status vector will be described here. + * + * REMEMBER: The value of a sensor in the status vector MUST be updated also if the sensor data didn't change + * +**/ + #ifdef STATUS_VECTOR_CPP #define sv_extr #else @@ -9,22 +21,22 @@ #endif #define dim 20 -#define CURRENT_DATA_READ (datas[(currentSVIndex-1) % dim]) -#define CURRENT_DATA_WRITE (datas[(currentSVIndex) % dim]) -#define CURRENT_INPUT_READ (inputs[(currentSVIndex-1) % dim]) -#define CURRENT_INPUT_WRITE (inputs[(currentSVIndex) % dim]) +#define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)] ) +#define CURRENT_DATA_WRITE ( datas[((currentSVIndex))] ) +#define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] ) +#define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] ) typedef struct input{ int IMUAngle, USfr, USsx, USdx, USrr, BT; - byte ballByte, cameraByte, lineByte; + byte ballByte, cameraByte, lineByte, xb, yb, xy, yy; bool SW_DX, SW_SX; }input; typedef struct data{ - int IMUAngle, ballAngle, ballDistance, cameraAngle, cameraDistance, + int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role; byte xb, yb, xy, yy, lineSeen, lineActive; - bool mate, ATKgoal, DEFgoal; + bool mate, ATKgoal, DEFgoal, ballSeen; }data; sv_extr input inputs[dim]; diff --git a/src/data_source_ball.cpp b/src/data_source_ball.cpp index 89c99aa..71fea22 100755 --- a/src/data_source_ball.cpp +++ b/src/data_source_ball.cpp @@ -8,12 +8,16 @@ DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(se } void DataSourceBall :: postProcess(){ - if((value & 0x01) == 1){ - distance = value; - ballSeen = distance > 1; - }else{ - angle = value * 2; - } + if((value & 0x01) == 1){ + distance = value; + ballSeen = distance > 1; + }else{ + angle = value * 2; + } + CURRENT_INPUT_WRITE.ballByte = value; + CURRENT_DATA_WRITE.ballAngle = angle; + CURRENT_DATA_WRITE.ballDistance = distance; + CURRENT_DATA_WRITE.ballSeen = ballSeen; } void DataSourceBall :: test(){ diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp index 049fef3..e6782be 100755 --- a/src/data_source_bno055.cpp +++ b/src/data_source_bno055.cpp @@ -17,10 +17,8 @@ void DataSourceBNO055::readSensor(){ if(millis() - lastTime > DATA_CLOCK){ imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); this->value = (int) euler.x(); - - CURRENT_INPUT_WRITE.IMUAngle = this->value; - CURRENT_DATA_WRITE.IMUAngle = this->value; - lastTime = millis(); } + CURRENT_INPUT_WRITE.IMUAngle = this->value; + CURRENT_DATA_WRITE.IMUAngle = this->value; } diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index a0dbecb..7780f6a 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -46,7 +46,7 @@ void DataSourceCameraConic :: readSensor(){ yAngle = (yAngle+360)%360; bAngle = (bAngle+360)%360; - int angleFix = compass->getValue() > 180 ? compass->getValue() - 360 : compass->getValue(); + int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; //Fixes with IMU yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ; @@ -66,6 +66,18 @@ void DataSourceCameraConic :: readSensor(){ count++; } } + //Important: update status vector + CURRENT_INPUT_WRITE.cameraByte = value; + CURRENT_DATA_WRITE.xb = true_xb; + CURRENT_DATA_WRITE.yb = true_yb; + CURRENT_DATA_WRITE.xy = true_xy; + CURRENT_DATA_WRITE.yy = true_yy; + CURRENT_DATA_WRITE.yAngle = yAngle; + CURRENT_DATA_WRITE.bAngle = bAngle; + CURRENT_DATA_WRITE.yAngleFix = yAngleFix; + CURRENT_DATA_WRITE.bAngleFix = bAngleFix; + CURRENT_DATA_WRITE.yDist = yDist; + CURRENT_DATA_WRITE.bDist = bDist; } } diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index a0a8373..53b9b5c 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -76,8 +76,9 @@ void DriveController::drive(int dir, int speed, int tilt){ speed4 = -(speed2); // calcola l'errore di posizione rispetto allo 0 - delta = (compass->getValue()-tilt+360)%360; -; +// delta = (compass->getValue()-tilt+360)%360; + delta = (CURRENT_DATA_READ.IMUAngle-tilt+360)%360; + setpoint = 0; pid->SetControllerDirection(REVERSE); diff --git a/src/goalie.cpp b/src/goalie.cpp index 1787c64..0b0c7b7 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -21,24 +21,24 @@ void Goalie::realPlay(){ else drive->prepareDrive(0,0,0); } +int dir, degrees2; void Goalie::goalie(int plusang) { if(ball->distance < 185) drive->prepareDrive(ball->angle, 350, 0); else{ if(ball->angle > 340 || ball->angle < 20) plusang -= 20; - if(ball->angle > 180) ball->degrees2 = ball->angle - 360; - else ball->degrees2 = ball->angle; + if(ball->angle > 180) degrees2 = ball->angle - 360; + else degrees2 = ball->angle; - if(ball->degrees2 > 0) ball->dir = ball->angle + plusang; //45 con 8 ruote - else ball->dir = ball->angle - plusang; //45 con 8 ruote + if(degrees2 > 0) dir = ball->angle + plusang; //45 con 8 ruote + else dir = ball->angle - plusang; //45 con 8 ruote - if(ball->dir < 0) ball->dir = ball->dir + 360; - else ball->dir = ball->dir; - ball->b = ball->dir; + if(dir < 0) dir = dir + 360; + else dir = dir; storcimentoPorta(); - if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc); + if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc); else { - drive->prepareDrive(ball->dir, 350, 0); + drive->prepareDrive(dir, 350, 0); cstorc = 0; } } diff --git a/src/main.cpp b/src/main.cpp index 0459db1..f947985 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,6 +22,8 @@ void loop() { goalie->play(role==1); keeper->play(role==0); + Serial.println(CURRENT_DATA_READ.IMUAngle); + // Last thing to do: movement and update status vector drive->drivePrepared(); updateStatusVector(); diff --git a/src/status_vector.cpp b/src/status_vector.cpp index 3c67ec1..dee265e 100644 --- a/src/status_vector.cpp +++ b/src/status_vector.cpp @@ -4,7 +4,7 @@ void initStatusVector(){ currentSVIndex = 0; - for(int i=0; i>=dim; i++){ + for(int i=0; i>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf roi = (0, 6, 318, 152) @@ -57,11 +52,7 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+2) sensor.set_saturation(+1) -<<<<<<< HEAD sensor.set_brightness(-3) -======= -sensor.set_brightness(-2) ->>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf sensor.set_quality(0) sensor.set_auto_exposure(False, 6000) sensor.set_auto_gain(True) @@ -84,11 +75,11 @@ while(True): blue_led.off() - tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata + tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata 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=150, area_threshold=150, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=150, merge = True): img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy()) diff --git a/utility/OpenMV/main_test_conic.py.autosave b/utility/OpenMV/main_test_conic.py.autosave deleted file mode 100644 index f2bfe21..0000000 --- a/utility/OpenMV/main_test_conic.py.autosave +++ /dev/null @@ -1,117 +0,0 @@ -# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020 -# Based on: -# color tracking - By: paolix - ven mag 18 2018 - -# Automatic RGB565 Color Tracking Example -# -import sensor, image, time, pyb, math - -from pyb import UART -uart = UART(3,19200, timeout_char = 1000) - - -# LED Setup ################################################################## - -red_led = pyb.LED(1) -green_led = pyb.LED(2) -blue_led = pyb.LED(3) - -red_led.off() -green_led.off() -blue_led.on() -############################################################################## - - -#thresholds = [ (30, 100, 15, 127, 15, 127), # generic_red_thresholds -# (30, 100, -64, -8, -32, 32), # generic_green_thresholds -# (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds - -#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal -# (30, 45, 1, 40, -60, -19)] # thresholds blue goal -# -thresholds = [ (30, 70, -12, 19, 41, 68) , # thresholds yellow goal - (0, 70, -2, 34, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) - -roi = (0, 6, 318, 152) - -# Camera Setup ############################################################### -'''sensor.reset() -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QVGA) -sensor.skip_frames(time = 2000) -sensor.set_auto_gain(False) # must be turned off for color tracking -sensor.set_auto_whitebal(False) # must be turned off for color tracking -sensor.set_auto_exposure(False, 10000) -#sensor.set_backlight(1) -#sensor.set_brightness(+2) -#sensor.set_windowing(roi) -clock = time.clock()''' - -sensor.reset() -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QQVGA) -sensor.set_contrast(+2) -sensor.set_saturation(+1) -sensor.set_brightness(-3) -sensor.set_quality(0) -sensor.set_auto_exposure(False, 6000) -sensor.set_auto_gain(True) -sensor.skip_frames(time = 300) - -clock = time.clock() -############################################################################## - - - -# [] list -# () tupla - -'''while(True): - clock.tick() - img = sensor.snapshot()''' - -while(True): - clock.tick() - - blue_led.off() - - tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata - 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=100, area_threshold=150, merge = True): - img.draw_rectangle(blob.rect()) - img.draw_cross(blob.cx(), blob.cy()) - - if (blob.code() == 1): - tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - if (blob.code() == 2): - tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - - tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste - tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste - - ny = len(tt_yellow) - nb = len(tt_blue) - - '''Yellow''' - area,cx,cy,code = tt_yellow[ny-1] # coordinata x del piu' grande y se montata al contrario - cx = img.width() / 2 - cx - cy = img.height() / 2 - cy - angle = math.pi/2 - math.atan2(cy, cx) - dist = math.sqrt(cx*cx + cy*cy) - string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(angle)+" | "+str(dist)+str(area)+"y" - print (string_yellow) # test on serial terminal - - '''Blue''' - area,cx,cy,code = tt_blue[nb-1] # coordinata x del piu' grande y se montata al contrario - cx = img.width() / 2 - cx - cy = img.height() / 2 - cy - angle = math.pi/2 - math.atan2(cy, cx) - dist = math.sqrt(cx*cx + cy*cy) - string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(angle)+" | "+str(dist)+str(area)+"b" - print (string_blue) # test on serial terminal - - #print ("..................................") - -print(clock.fps())