Team Design project 3 main file
Dependencies: LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed
Diff: main.cpp
- Revision:
- 26:7b67bcbddde3
- Parent:
- 24:ecc3fbaf2824
--- a/main.cpp Mon Mar 23 12:38:53 2015 +0000
+++ b/main.cpp Mon Mar 23 14:04:24 2015 +0000
@@ -28,8 +28,8 @@
mode driveMode; //declaring the variable for the states
int sensorsCheckSum; //varibale used for sensors mapping access
int passedTime1,passedTime2;
-int values_old[5] = {0};
-int cursor = 0;
+int oldValues[5] = {0};
+int cursor = 0, k = 0;
void measureSensors () {
@@ -44,8 +44,8 @@
sensorsCheckSum += (i+1)*(i+1);
}
}
- if (values_old[cursor] != sensorsCheckSum) { //why only if different ??
- values_old[cursor] = sensorsCheckSum;
+ if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
+ oldValues[cursor] = sensorsCheckSum;
cursor = (cursor+1)%5;
}
//pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
@@ -103,7 +103,7 @@
wait(1); //give time to set up the system
sensorTimer.start(); // start timer for sensors
- float normalSpeed = 0.01f;
+// float normalSpeed = 0.01f;
while(1){
if (pc.getc() == 'r') {
@@ -127,118 +127,134 @@
// motors.setSpeed(normalSpeed);
// }
- int testOtherCases = 0; //needed for control statements
+// int testOtherCases = 0; //needed for control statements
while (1) {
-
- if (driveMode == REGULAR) {
- measureSensors();
- switch (sensorsCheckSum) {
- case 0: // all black, turn around by 180 degrees or a possible turn on the left
- for (k=0;k<6;k++) { //left turn situation
- if (oldValues[k] == 194) {
- motors.stop();
- motors.setSteeringMode(NORMAL);
- motors.reverse();
- motors.setSpeed(0.1f);
- motors.start();
- wait(1);
- motors.stop();
- } else {
- motors.stop();
- motors.setSteeringMode(NORMAL);
- motors.reverse();
- motors.setSpeed(0.1f);
- motors.start();
- wait(2);
- motors.stop();
- motors.setSteeringMode(PIVOT_CCW);
- motors.setSpeed(0.1f);
- do
- {
- motors.start();
- measureSensors();
- } while (sensorsCheckSum != 96);
- motors.stop();
- motors.setSteeringMode(NORMAL);
- }
+// if (driveMode == REGULAR) {
+// measureSensors();
+ switch (sensorsCheckSum) {
+ case 0: // all black, turn around by 180 degrees or a possible turn on the left
+ for (k=0;k<6;k++) { //left turn situation >> stop, go backwards
+ if (oldValues[k] == 194) {
+ motors.stop();
+ motors.setSteeringMode(NORMAL);
+ motors.reverse();
+ motors.setSpeed(0.1f);
+ motors.start();
+ wait(2);
+ motors.stop();
+ } else { //dead end situation >> stop, go a bit backwards so there is space for turning and turn CCW
+ motors.stop();
+ motors.setSteeringMode(NORMAL);
+ motors.reverse();
+ motors.setSpeed(0.1f);
+ motors.start();
+ wait(2);
+ motors.stop();
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(0.1f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum != 96);
+ motors.stop();
+ }
+ }
break;
- case 30: //all right are white, left all black >> turn right(move left wheel)
+ case 30: //all right are white, left all black >> turn right(move left wheel faster)
motors.setSteeringMode(NORMAL);
motors.forward();
- motors.setRightSpeed(0.15f);
- motors.setLeftSpeed(0.5f);
+ motors.setRightSpeed(0.1f);
+ motors.setLeftSpeed(0.05f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum == 30);
+ motors.stop();
break;
-// case 46: //left 5 white, right only 3 black >> turn right
-// motors.setRightSpeed(0.15f);
-// motors.setLeftSpeed(0.1f);
-// break;
+
case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
motors.setSteeringMode(NORMAL);
motors.forward();
motors.setSpeed(0.1f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum == 94);
+ motors.start();
+ break;
+ case 104: //right all white, left half white >> turn right
+ motors.setSteeringMode(NORMAL);
+ motors.forward();
+ motors.setSpeed(0.1f);
motors.start();
- break;
- case 104: //right all white, left half white >> turn right
- motors.setRightSpeed(0.1f);
- motors.setLeftSpeed(0.15f);
+ wait(1);
+ motors.stop();
+ motors.setRightSpeed(0.2f);
+ motors.setLeftSpeed(0.0f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum <= 94);
+ motors.stop();
break;
-// case 154: //right 4 white, left only 6 black >> turn left
-// break;
case 174: //left all white, right all black >> turn left (move right wheel)
+ motors.setSteeringMode(NORMAL);
+ motors.forward();
motors.setRightSpeed(0.05f);
- motors.setLeftSpeed(0.15f);
- break;
- case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
+ motors.setLeftSpeed(0.1f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum == 174);
break;
- case 204 : //all sensors are white
-
- for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
- if (oldValues[k] == 194) { //checks whether black line on the right was present before
- motors.setRightSpeed(0.15f);
- motors.setLeftSpeed(0.05f);
- }
-
- if (oldValues[k] == 104) { //right turn 90 situation
- motors.stop();
- motors.setSteeringMode(PIVOT_CW);
- motors.setRightSpeed(0.1f);
- motors.setLeftSpeed(0.1f);
- do
- {
- motors.start();
- measureSensors();
- } while (sensorsCheckSum != 94);
- motors.stop();
- motors.setSteeringMode(NORMAL);
- }
+ case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
+ motors.setSteeringMode(NORMAL);
+ motors.forward();
+ motors.setRightSpeed(0.05f);
+ motors.setLeftSpeed(0.1f)
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum == 194);
+ motors.stop(); //do while left is white
+ break;
+ case 204 : //all sensors are white
+ for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
+ if (oldValues[k] == 194) { //checks whether black line on the right was present before
+ motors.setSteeringMode(NORMAL);
+ motors.setRightSpeed(0.05f);
+ motors.setLeftSpeed(0.1f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum == 204);
+ motors.stop();
+ }
+ if (oldValues[k] == 104) { //right turn 90 situation
+ motors.setSteeringMode(PIVOT_CW);
+ motors.setRightSpeed(0.1f);
+ motors.setLeftSpeed(0.1f);
+ do
+ {
+ motors.start();
+ measureSensors();
+ } while (sensorsCheckSum != 94);
+ motors.stop();
+ }
break;
-
- default: //checksum is zero , all are black
+ default:
measureSensors();
break;
}
}
-// if (testOtherCases == 1) {
-// if (sensorsCheckSum < 96){ // adjust right
-// } else {//adjust left
-// }
-// testOtherCases = 0;
-// } else { //if (driveMode == SQUARE} //implement the square searching thing.
- }
- }
-
- if (testOtherCases == 1) {
- if (sensorsCheckSum < 96){ // adjust right
- }
- else {//adjust left
- }
- testOtherCases = 0;
- }
-// }
-// else { //if (driveMode == SQUARE}
-// //implement the square searching thing..
-
-
-
-
-
+}
+}
+}
+}