diff options
Diffstat (limited to 'Venus_Skeleton/Venus_Skeleton.ino')
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 100 |
1 files changed, 94 insertions, 6 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 213f0e6..531235c 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -83,6 +83,7 @@ int currValServoTurret = 0; // Actuator values int currValTurret = 0; int currValDirection = 0; +int currValDirectionDegree = 0; // Timers long timerMovementStart = 0; @@ -95,6 +96,7 @@ int counterInit = 0; int counterMovement = 0; int counterTurret = -1; int counterTurretWait = 0; +int counterSampleConfirm = 0; // ********************** @@ -463,6 +465,66 @@ bool moveTurnTo(int direction) { maneuver(100,-100); } } else { + if(direction < NUM_DIRECTIONS){ + int sign = -1; + } + if(direction >= NUM_DIRECTIONS){ + int sign = 1; + } + int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360; + data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360; + } + + if (timerMovementStop < millis()) { + stopMovement(); + currValDirection = 0; + for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) { + data.obstacle_turret_distances[k] = 300; + } + } +} + +void moveTurnToDeg(int degree) { + if (degree == 0 || degree == NUM_DIRECTIONS) { + return true; + } + + // Make sure turret doesn't move + timerTurret = 1; + + // if movementtimer is not started, start timer or something and initiate movement + // else continue movement and return false + // if movementtimer is expired, stop movement. + + if (!timerMovementStop) { + DEBUG_PRINT(" Start turn move: "); + DEBUG_PRINT(degree); + DEBUG_PRINT("deg - "); + counterMovement++; + // timer = now() + direction * formula + timerMovementStart = millis(); + timerMovementStop = millis() + (degree % NUM_DIRECTIONS) * CAL_MOVE_TURN; + + // DO A COMPASS CHECK + + maneuver(0,0); + if(degree < 0){ + DEBUG_PRINT(" left\n"); + // turn left + maneuver(-100,100); + } + if(degree > 0){ + DEBUG_PRINT(" right\n"); + // Turn right + maneuver(100,-100); + } + } else { + if(degree < 0){ + int sign = -1; + } + if(degree > 0){ + int sign = 1; + } int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360; data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360; } @@ -593,7 +655,7 @@ int checkBestRoute(int preferedDirection) { if (preferedDirection == -1) { directionArray[i] = 1; } else { - directionArray[i] = 100 + abs((int)(100 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2))); + directionArray[i] = 1 + abs((int)(pow(2, abs((int)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2))))); } // limit to first 180deg @@ -688,12 +750,26 @@ void checkSample() { stopAllServos(); sensSampleTurret(); sensSampleGripper(); + sensObstacleTurret(); } } -bool confirmSample() { - if (data.sample_turret_detected || data.sample_gripper_detected) { - if (!data.sample_gripper_detected) { +bool confirmSample(int turretVal) { + if (!counterSampleConfirm) { + + // Turret direction in degrees calculated from forward. (negative is left, positive is right) + int checkTurretDeg = (int)((counterTurret - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1); + + + int checkTurretDegAbs = 90 - abs(checkTurretDeg); + int currValDirectionDegree = + moveTurnToDeg + } + + + if (data.sample_turret_detected[turretVal] || data.sample_gripper_detected[turretVal]) { + if (!data.sample_gripper_detected[turretVal]) { + // Turn robot to sample // If still no sample_gripper_detected return false } @@ -767,7 +843,7 @@ void setup() { void loop() { - //sensCompass(); + // x sensCompass(); sensObstacleTurret(); sensIRLine(L); sensIRLine(R); @@ -776,7 +852,6 @@ void loop() { //sensLab(); - //checkSample(); checkFreePath(); // calculateOrientation(); @@ -811,6 +886,7 @@ void loop() { break; case OPMODE_MAPPING: + checkSample(); turnTurretToNext(); checkBestRoute(0); moveTurnTo(currValDirection); @@ -818,6 +894,18 @@ void loop() { break; case OPMODE_CHECKSAMPLE: + + + + + confirmSample(); + moveTurnToDeg(); + currValTurret + + + + + break; case OPMODE_GRABSAMPLE: |