diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h index 59dce44..cc4251d 100644 --- a/include/positionsys_camera.h +++ b/include/positionsys_camera.h @@ -1,7 +1,17 @@ +#include "PID_v2.h" #include "systems.h" -#define CAMERA_CENTER_X 3 -#define CAMERA_CENTER_Y 6 +#define CAMERA_CENTER_X 0 +#define CAMERA_CENTER_Y_BOTH 3 +#define CAMERA_CENTER_Y_BLUE -60 +#define CAMERA_CENTER_Y_YELLOW -30 + +#define Kpx 1 +#define Kix 0 +#define Kdx 0 +#define Kpy 1 +#define Kiy 0 +#define Kdy 0 class PositionSysCamera : public PositionSystem{ @@ -10,5 +20,12 @@ class PositionSysCamera : public PositionSystem{ void goCenter(); void update() override; void test() override; + void setCameraPID(); + void CameraPID(); + + double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; + + PID* X; + PID* Y; }; diff --git a/lib/Arduino-PID-Library b/lib/Arduino-PID-Library new file mode 160000 index 0000000..9b4ca0e --- /dev/null +++ b/lib/Arduino-PID-Library @@ -0,0 +1 @@ +Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index a25b3d1..87c47a5 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -50,7 +50,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) vyn = 0; } -void DriveController::prepareDrive(int dir, int speed, int tilt=0){ +void DriveController::prepareDrive(int dir, int speed, int tilt){ pDir = dir; pSpeed = speed; pTilt = tilt; diff --git a/src/main.cpp b/src/main.cpp index ca69d9f..077a193 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,6 +25,7 @@ void loop() { // Last thing to do: movement and update status vector // drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix); + drive->drivePrepared(); updateStatusVector(); } diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp index 765e0c7..1a63c3b 100644 --- a/src/positionsys_camera.cpp +++ b/src/positionsys_camera.cpp @@ -3,7 +3,9 @@ #include "vars.h" #include "sensors.h" -PositionSysCamera::PositionSysCamera() {} +PositionSysCamera::PositionSysCamera() { + setCameraPID(); +} void PositionSysCamera::update(){ } @@ -14,26 +16,83 @@ void PositionSysCamera::test(){ void PositionSysCamera::goCenter(){ /*WORKS BUT CAN BE BETTER*/ //Y - if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); + /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); //X else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); - else drive->prepareDrive(0, 0, 0); + else drive->prepareDrive(0, 0, 0); */ /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/ - // int x = 1; - // int y = 1; + /* int x = 1; + int y = 1; - // //Trying using an angle - // if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) - // y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; - // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; - // if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; + //Trying using an angle + if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ + if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) + y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; + if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; + if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; - // int dir = -90-(atan2(y*1.5,x)*180/3.14); - // dir = (dir+360) % 360; - // drive->prepareDrive(dir, 100, 0); + int dir = -90-(atan2(y*1.5,x)*180/3.14); + dir = (dir+360) % 360; + drive->prepareDrive(dir, 100, 0); + } */ + CameraPID(); +} +//using a pid controller for the movement, or trying at least +void PositionSysCamera :: setCameraPID(){ + Inputx = 0; + Outputx = 0; + Setpointx = 0; + Inputy = 0; + Outputy = 0; + Setpointy = 0; + + X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT); + X->SetOutputLimits(-50,50); + X->SetMode(AUTOMATIC); + X->SetDerivativeLag(1); + X->SetSampleTime(2); + Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT); + Y->SetOutputLimits(-50,50); + Y->SetMode(AUTOMATIC); + Y->SetDerivativeLag(1); + Y->SetSampleTime(2); +} + +void PositionSysCamera :: CameraPID(){ + if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ + Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2; + Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; + Setpointx = CAMERA_CENTER_X; + Setpointy = CAMERA_CENTER_Y_BOTH; + } + if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){ + Inputx = CURRENT_DATA_READ.cam_xb; + Inputy = CURRENT_DATA_READ.cam_yb; + Setpointx = CAMERA_CENTER_X; + Setpointy = CAMERA_CENTER_Y_BLUE; + } + if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){ + Inputx = CURRENT_DATA_READ.cam_xy; + Inputy = CURRENT_DATA_READ.cam_yy; + Setpointx = CAMERA_CENTER_X; + Setpointy = CAMERA_CENTER_Y_YELLOW; + //Setpointy todo + }else{ + + } + //TODO: no goal seen + + X->Compute(); + Y->Compute(); + + // DEBUG.println(Outputx); + + int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14); + dir = (dir+360) % 360; + drive->prepareDrive(dir, 100, 0); } \ No newline at end of file diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 1bd4edf..5c54f24 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal - (39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (23, 38, -34, -8, -12, 22), # thresholds yellow goal + (69, 98, -14, 30, 66, 113)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -60,10 +60,11 @@ sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+3) -sensor.set_saturation(0) -sensor.set_brightness(-2) +sensor.set_saturation(2) +sensor.set_brightness(0) sensor.set_quality(0) -sensor.set_auto_exposure(False, 6000) +sensor.set_auto_whitebal(False) +sensor.set_auto_exposure(False, 5000) sensor.set_auto_gain(True) sensor.skip_frames(time = 300)