Dependencies: LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed
Diff: main.cpp
- Revision:
- 25:bec5dc4c9f5e
- Parent:
- 24:ecc3fbaf2824
- Child:
- 27:fc0fd2c0eebb
--- a/main.cpp Mon Mar 23 12:38:53 2015 +0000
+++ b/main.cpp Mon Mar 23 14:03:29 2015 +0000
@@ -28,7 +28,7 @@
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 oldValues[5] = {0};
int cursor = 0;
@@ -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);
@@ -65,36 +65,6 @@
Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
- // motors.setSpeed(0.1f);
-// motors.forward();
-// motors.start();
-// wait(2);
-// float x=0.1f;
-// while (1) {
-// motors.setLeftSpeed(x);
-// x = x+0.05;
-// wait(3);
-// }
-// motors.setLeftSpeed(0.1f);
-// wait(5);
-// motors.setLeftSpeed(0.2f);
-// motors.setRightSpeed (0.2f);
-// wait(3);
-// motors.setRightSpeed (0.1f);
-// wait(5);
-// motors.stop();
-
- //wait(1);
-// motors.reverse();
-// wait(5);
-// motors.stop();
-// motors.setSpeed(0.5f);
-// motors.start();
-// wait(5);
-// motors.stop();
-// wait(1);
-// motors.reverse();
-
// setup_counter(1000, 0);
// float frequency = measure_frequency(CHANNEL_1);
measureTimer.start();
@@ -104,7 +74,7 @@
sensorTimer.start(); // start timer for sensors
float normalSpeed = 0.01f;
-
+/*
while(1){
if (pc.getc() == 'r') {
measureSensors();
@@ -126,119 +96,111 @@
// else {
// motors.setSpeed(normalSpeed);
// }
-
+ }
+*/
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);
- }
- break;
- case 30: //all right are white, left all black >> turn right(move left wheel)
- motors.setSteeringMode(NORMAL);
- motors.forward();
- motors.setRightSpeed(0.15f);
- motors.setLeftSpeed(0.5f);
- 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);
- motors.start();
- break;
- case 104: //right all white, left half white >> turn right
- motors.setRightSpeed(0.1f);
- motors.setLeftSpeed(0.15f);
- 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.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
- 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);
- }
- break;
+ if (driveMode == REGULAR) {
+ measureSensors();
+ switch (sensorsCheckSum) {
+ case 0: // all black, turn around by 180 degrees or a possible turn on the left
+ for (int k=0;k<5;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);
+ }
+ }
+ break;
+ case 30: //all right are white, left all black >> turn right(move left wheel)
+ motors.setSteeringMode(NORMAL);
+ motors.forward();
+ motors.setRightSpeed(0.15f);
+ motors.setLeftSpeed(0.5f);
+ 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);
+ motors.start();
+ break;
+ case 104: //right all white, left half white >> turn right
+ motors.setRightSpeed(0.1f);
+ motors.setLeftSpeed(0.15f);
+ 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.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
+ break;
+ case 204 : //all sensors are white
+
+ for (int k=0;k<5;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);
+ }
+ break;
+ default: //checksum is zero , all are black
+ testOtherCases = 1;
+ break;
+ }
- default: //checksum is zero , all are black
- measureSensors();
- break;
+ if (testOtherCases == 1) {
+ if (sensorsCheckSum < 96){ // adjust right
+ }
+ else {//adjust left
+ }
+ testOtherCases = 0;
+ }
+
+ }
+ else {// if driveMode == SQUARE)
}
}
-// 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..
+}
-
-
-
-