diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index 6d1d526..af2653d 100644 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -28,6 +28,10 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) errorP = 0; errorI = 0; errorD = 0; + + errorePre = 0; + pidfactor = 0; + integral = 0; } void DriveController::prepareDrive(int dir, int speed, int tilt){ diff --git a/src/ds_ctrl_lines.cpp b/src/ds_ctrl_lines.cpp index c3672c4..f973b5c 100644 --- a/src/ds_ctrl_lines.cpp +++ b/src/ds_ctrl_lines.cpp @@ -50,7 +50,7 @@ void DataSourceCtrlLines::read(){ } void DataSourceCtrlLines::postProcess(){ - if ((inV > 0) || (outV > 0)) { + if ((inV > 0) || (outV > 0)) { fboundsOX = true; fboundsOY = true; if(exitTimer > EXTIME) { @@ -64,7 +64,7 @@ void DataSourceCtrlLines::postProcess(){ } void DataSourceCtrlLines::outOfBounds(){ - //handleExtern(); + handleExtern(); handleIntern(); } @@ -144,8 +144,8 @@ void DataSourceCtrlLines::handleIntern(){ break; } - if(exitTimer < 45) outVel = 200; - else outVel = 150; + if(exitTimer < 45) outVel = 350; + else outVel = 330; drive->prepareDrive(outDir, outVel, 0); // keeper_backToGoalPost = true; // keeper_tookTimer = true; @@ -154,7 +154,7 @@ void DataSourceCtrlLines::handleIntern(){ inVOldX = 0; inVOldY = 0; // lineSensByteBak = 30; - //drive->canUnlock = true; + drive->canUnlock = true; } // lineSensByteBak = linesensbyteI; diff --git a/src/goalie.cpp b/src/goalie.cpp index 3d0cfe6..d86e499 100644 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -73,7 +73,9 @@ drive->prepareDrive(atk_direction, atk_speed, 0); } else drive->prepareDrive(atk_direction, atk_speed, 0); - /* digitalWrite(LED_G, HIGH); + + + /* siria digitalWrite(LED_G, HIGH); if(ball->ballSeen){ if(ball->angle >= 0 && ball->angle < 45) drive->drive(ball->angle, 75, 0); else if(ball->angle >= 45 && ball->angle <= 60) drive->drive(ball->angle, 75, 0); @@ -88,7 +90,7 @@ void Goalie::ballBack(){ int ballangle2; int dir; int plusang; - + if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; else plusang = 0; diff --git a/src/main.cpp b/src/main.cpp index 9324dd1..6e13ba8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -87,6 +87,7 @@ void setup() { delay(2000); } + void loop() { updateSensors(); Trial trial; @@ -94,6 +95,9 @@ void loop() { //drive->prepareDrive(0,0,0); //linesCtrl->update(); + + //drive->prepareDrive(0,0,0); + linesCtrl->update(); drive->drivePrepared(); }