summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorPeter de Kok <p.j.s.d.kok@gmail.com>2015-06-10 18:35:42 +0200
committerPeter de Kok <p.j.s.d.kok@gmail.com>2015-06-10 18:35:42 +0200
commita8b4777f6f395a4d0576bdccaee4704e22d23d01 (patch)
treeff2f862a934e9c035b80f3092ab970c753eb4c99
parent8d056e611e9d2475c7e0bc16614befbdcad3367c (diff)
downloadcode-a8b4777f6f395a4d0576bdccaee4704e22d23d01.tar.gz
check Sample & confirm Sample created
-rw-r--r--Venus_Skeleton/Venus_Skeleton.ino70
1 files changed, 50 insertions, 20 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index c9481e2..5873726 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -520,6 +520,7 @@ void moveTurnToDeg(int degree) {
if (timerMovementStop < millis()) {
stopMovement();
+ currValDirectionDegree = 0;
for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
data.obstacle_turret_distances[k] = 300;
}
@@ -598,6 +599,9 @@ void opMode(opmode_t opmode) {
DEBUG_PRINT("\nOPMODE");
DEBUG_PRINT(opmode);
DEBUG_PRINT("\n\n");
+ if(operationMode != opmode) {
+ counterSampleConfirm = 0;
+ }
operationMode = opmode;
counterTurretWait = 0;
}
@@ -732,10 +736,14 @@ int eliminateDirections(int directionArray[]) {
}
void checkSample() {
- if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected || data.sample_gripper_detected)) {
+ if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) && (data.obstacle_turret_distances[counterTurret] > 37)) {
// One of the turret sees a sample (or the lab)
opMode(OPMODE_CHECKSAMPLE);
- stopAllServos();
+
+ // If sample straight ahead, set counterTurret to 6
+ if (data.sample_gripper_detected) {
+ counterTurret = 3;
+ }
sensSampleTurret();
sensSampleGripper();
sensObstacleTurret();
@@ -744,21 +752,41 @@ void checkSample() {
bool confirmSample(int turretVal) {
if (!counterSampleConfirm) {
-
+ stopAllServos();
// 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 checkTurretDeg = (int)((turretSequence[counterTurret] - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1);
+ int sign = checkTurretDeg < 0 ? -1 : 1;
+ // Calculate angle the robot needs to turn to point in direction of possible sample.
int checkTurretDegAbs = 90 - abs(checkTurretDeg);
- int x = CAL_SAMPLE_TURRET_MAX_DIST * sin((checkTurretDegAbs/180.0)*PI);
- int y = CAL_SAMPLE_TURRET_MAX_DIST * cos((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST;
-
- int currValDirectionDegree = (int)((atan2(x, y)/PI) * 180.0 ) + 10;
+ int x = CAL_SAMPLE_TURRET_MAX_DIST * cos((checkTurretDegAbs/180.0)*PI);
+ int y = CAL_SAMPLE_TURRET_MAX_DIST * sin((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST;
+ int currValDirectionDegree = (int)((atan2(x, y)/PI) * 180.0);
+ currValDirectionDegree += (x == 0 ? 0 : 10);
+ currValDirectionDegree *= sign;
counterSampleConfirm++;
} else {
- if (data.sample_gripper_detected) {
+ // Check if a sample was seen by the gripperSampleDetector and the movement to drive the grippers over it are done
+ if (!counterMovement && counterSampleConfirm == 2) {
+ // Pick it up
+ opMode(OPMODE_GRABSAMPLE);
+ }
+ // Check if a sample is seen by the gripperSampleDetector
+ if (data.sample_gripper_detected && (counterSampleConfirm == 1 || counterSampleConfirm == 3)) {
stopMovement();
currValDirectionDegree = 0;
+ counterSampleConfirm = 2;
+ }
+ // Check if movement has stopped (turn and forward), without seeing a sample (second turn). -> False positive
+ if (!counterMovement && counterSampleConfirm == 3) {
+ opMode(OPMODE_WAIT);
+ }
+ // Check if movement has stopped (turn and forward), without seeing a sample (first turn). -> Turn other way
+ if (!counterMovement && counterSampleConfirm == 1) {
+ int sign = currValDirectionDegree < 0 ? 1 : -1;
+ currValDirectionDegree = 60 * sign;
+ counterSampleConfirm = 3
}
}
}
@@ -837,7 +865,8 @@ void loop() {
//sensSampleGripper();
//sensLab();
-
+
+ checkSample();
checkFreePath();
// calculateOrientation();
@@ -872,7 +901,6 @@ void loop() {
break;
case OPMODE_MAPPING:
- checkSample();
turnTurretToNext();
checkBestRoute(0);
moveTurnTo(currValDirection);
@@ -880,24 +908,26 @@ void loop() {
break;
case OPMODE_CHECKSAMPLE:
-
-
-
-
+ // Sequence:
+ // - Turn a little further then turret direction or until sample detected
+ // - Drive forward 15 cm (either to pick up, or try a little further)
+ // - Turn other way for 60deg or until sample detected
+ // - Drive forward 15 cm (either to pick up, or to finish sequence)
+ // - Either a false positive or a success
confirmSample(counterTurret);
+ turnTurretToNext();
moveTurnToDeg(currValDirectionDegree);
- moveStraight(currValDirectionDegree, 10.0);
-
-
-
-
+ moveStraight(currValDirectionDegree, 15.0);
break;
case OPMODE_GRABSAMPLE:
+ // Close grippers to calibration value (sample sensor should not see grippers)
break;
case OPMODE_GOTOLABLOCATION:
sensBeaconTurret();
+ // check if sample still there.
+ // calculate prefered direction to (0, 0)
break;
case OPMODE_WAITFORLAB: