Ivelin Kozarev
Team Design project 3 main file
Dependencies: LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed
Diff: main.cpp
- Revision:
- 37:3d14df6dec34
- Parent:
- 36:a48d57d63630
--- a/main.cpp Wed Mar 25 11:50:44 2015 +0000
+++ b/main.cpp Thu Mar 26 20:13:49 2015 +0000
@@ -27,6 +27,7 @@
void turnRight ();
void measureSensors ();
void printBluetooth();
+void shootFind ();
Timer measureTimer; //Timer used for measurement
@@ -39,13 +40,15 @@
int oldValues[5] = {0};
int cursor = 0, k = 0;
static float normalSpeed = 0.15f;
+int countRightTurns = 0;
+int countLeftTurns = 0;
int main() {
//setup_counter(1000, 0);
// float frequency = measure_frequency(CHANNEL_1);
- led = 0;//start LED with beginning of main program
+ led = 1;//start LED with beginning of main program
measureTimer.start();
driveMode = REGULAR; //initialise drivemoder
@@ -54,86 +57,131 @@
sensorTimer.start(); // start timer for sensors
- motors.setSteeringMode (NORMAL);
- motors.disableHardBraking();
- motors.forward ();
-
- motors.start ();
- motors.setSpeed (normalSpeed);
-
+ squareOut.period(0.01f); // 0.01 second period
+ squareOut.write(0.5); // 50% duty cycle
+// for (int b = 1; b <7;b++) {
+// shoot(0.4);
+// }
+ shoot (0);
+// motors.setSteeringMode (NORMAL);
+// motors.disableHardBraking();
+// motors.forward ();
+//
+// motors.start ();
+//
+// motors.setSpeed (normalSpeed);
+
//motors.enableSlew();
-
+
// HC06.printf("heello");
int testOtherCases = 0; //used for control
while(1){
- measureSensors();
- //printBluetooth();
+
+ // printBluetooth();
// while (HC06.getc() != 'r') {
// measureSensors();
// printBluetooth();
// wait(1);
// }
+
+
+ measureSensors();
+ printBluetooth();
switch (sensorsCheckSum) {
+
+
case 94:
motors.setSpeed (normalSpeed);
- wait(0.4);
+ wait(0.15);
break;
case 104:
- motors.setSpeed (normalSpeed);
- wait(0.15);
- measureSensors();
-
- if (sensorsCheckSum == 104) { //confirm the turn
-
- wait(0.15);
- measureSensors();
- if (sensorsCheckSum == 104)
- turnRight ();
- }
+ if ( countRightTurns == 1)
+ turnRight();
+ else {
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 104) { //confirm the turn
+
+ wait(0.15);
+ measureSensors();
+ if (sensorsCheckSum == 104)
+ turnRight ();
+ }
+ }
break;
+ case 158:
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
+
+ turnLeft();
+ }
+ break;
case 168:
- motors.setSpeed (normalSpeed);
- wait(0.15);
- measureSensors();
-
- if (sensorsCheckSum == 104) { //confirm the turn
+ if ( countRightTurns == 1)
+ turnRight();
+ else {
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
- wait(0.15);
- measureSensors();
- if (sensorsCheckSum == 104)
- turnRight ();
+ if (sensorsCheckSum == 168) { //confirm the turn
+
+ wait(0.15);
+ measureSensors();
+ if (sensorsCheckSum == 168)
+ turnRight ();
+ }
}
break;
+ case 194:
+ motors.setSpeed (normalSpeed);
+ wait(0.15);
+ measureSensors();
+
+ if (sensorsCheckSum == 194) { //confirm the turn
+
+
+ turnLeft();
+ }
+ break;
default:
testOtherCases =1;
break;
}
if (testOtherCases == 1) {
+
if (sensorsCheckSum < 96) {
motors.setSpeed (0.0);
- motors.setLeftSpeed(normalSpeed*2);
- wait(0.2);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.075);
}
else if (sensorsCheckSum > 96) {
motors.setSpeed (0.0);
- motors.setRightSpeed(normalSpeed*2);
- wait(0.2);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.1);
}
- }
+ }
+
motors.setSpeed(0.0);
wait(0.2);
+
}
} //int main
void printBluetooth() { //for debugging
HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
- //HC06.printf("%i %i %i %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->state);
+ //HC06.printf("%i %i %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
//HC06.printf("%i %i/n%i %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);
//HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
@@ -143,31 +191,210 @@
void turnRight () {
+
ledMode = 0; // put on LED to know we're in this state
- motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
- wait(0.3);
+ if (countRightTurns == 0 ) {
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1);
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (1);
+ }
+ else {
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1);
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (0.8);
+ }
+
+ int exit = 0; //used for exiting the function
motors.setSpeed(0.0);
- motors.setLeftSpeed (normalSpeed); //turn only the relevant wheel
- wait (1.7);
- int exit = 0; //used for exiting the function
-
- for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+ wait(0.5);
+ motors.setSteeringMode (NORMAL);
+ wait(0.3);
+ while (1) {
motors.setSpeed(0.0); //stop and check if the line is reached
exit = 0;
- if (measure(sensorArray[i]) == 0) {//if sensor is black
- exit ++;
+ wait (0.4);
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // two sensors are considered on line
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
+
+ if (measure(sensorArray[i]) == 0)
+ checkSumLocal += i*i;
+ }
+ printBluetooth();
+ if (checkSumLocal == 74) {
+ led = 0;
+ ledMode = 1; // turn off the LED
+ countRightTurns ++;
+ wait(1);
+ return;
+ }
+ if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.6);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+
+ }
+ else if (checkSumLocal < 74) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal > 74) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+ motors.setSpeed(0.0);
+ wait(0.4);
+ }
+
+ }
+
}
- if (exit > 1){ // two sensors are considered on line
- ledMode = 1; // turn off the LED
- return;
- }
- motors.setSpeed (normalSpeed);
+// motors.setSpeed(normalSpeed);
+ motors.setRightSpeed (normalSpeed*1.7);
+ motors.setLeftSpeed (normalSpeed);
wait(0.2);
}
}
-void turnLeft () { //TOBE implemented
+void turnLeft () {
+
+ ledMode = 0; // put on LED to know we're in this state
+ motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
+ wait(1.1);
+ motors.setSteeringMode(PIVOT_CW);
+ motors.setSpeed (normalSpeed); //turn only the relevant wheel
+ wait (1);
+ int exit = 0; //used for exiting the function
+ motors.setSpeed(0.0);
+ wait(0.5);
+ motors.setSteeringMode (NORMAL);
+ wait(0.3);
+ while (1) {
+ motors.setSpeed(0.0); //stop and check if the line is reached
+ exit = 0;
+ wait (0.4);
+ for (int i = 0; i < 4; i++) {
+
+ if (measure(sensorArray[i]) == 0) {//if sensor is black
+ exit ++;
+ }
+ if (exit > 2){ // two sensors are considered on line
+ motors.setSteeringMode(PIVOT_CW);
+ motors.setSpeed(normalSpeed);
+ wait (0.3);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+ while (1) {
+ int checkSumLocal =0;
+ for (int i = 0; i < 4; i++) {
+
+ if (measure(sensorArray[i]) == 1)
+ checkSumLocal += (i+1)*(i+1);
+ }
+ HC06.printf ("checkSumLocal is %i \n", checkSumLocal);
+ if (checkSumLocal == 20) {
+ led = 0;
+ ledMode = 1; // turn off the LED
+ countLeftTurns ++;
+ wait(1);
+ if (countLeftTurns == 1) {
+ //shootFind ();
+ }
+ return;
+ }
+ if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
+ motors.setSteeringMode(PIVOT_CCW);
+ motors.setSpeed(normalSpeed);
+ wait (0.6);
+ motors.setSpeed(0.0);
+ motors.setSteeringMode(NORMAL);
+
+ }
+ else if (checkSumLocal > 20) {
+ motors.setSpeed (0.0);
+ motors.setRightSpeed(normalSpeed);
+ wait(0.2);
+ }
+ else if (checkSumLocal < 20) {
+ motors.setSpeed (0.0);
+ motors.setLeftSpeed(normalSpeed);
+ wait(0.2);
+ }
+ motors.setSpeed(0.0);
+ wait(0.4);
+ }
+
+ }
+
+ }
+// motors.setSpeed(normalSpeed);
+ motors.setRightSpeed (normalSpeed);
+ motors.setLeftSpeed (normalSpeed*1.5);
+ wait(0.2);
+ }
+}
+
+void shootFind () {
+ motors.setSpeed(normalSpeed);
+ wait (1);
+ motors.setSteeringMode (PIVOT_CW);
+ wait (0.8);
+ motors.setSteeringMode (NORMAL);
+
+ while (1) {
+ motors.setSpeed(0.0);
+ wait (0.3);
+ for (int i = 8; i < 11 ; i++) {
+ if (measure(sensorArray[i]) == 2){
+ motors.setSteeringMode (PIVOT_CW);
+ motors.setSpeed(normalSpeed);
+ wait (1.6);
+ motors.setSpeed(normalSpeed);
+ for (int b = 1; b <7;b++) {
+ shoot(0.1*(float)b);
+ }
+ shoot (0);
+ motors.setSteeringMode(NORMAL);
+ // while (1) {
+// motors.setSpeed(0.0);
+// wait (0.4);
+// for (int c =4 ;c <8;c++) {
+// if( measure(sensorArray[i]) == 0){
+// return;
+// }
+// }
+// motors.setSpeed(normalSpeed);
+// motors.setLeftSpeed(normalSpeed*1.3);
+// wait(0.2);
+// }
+ return;
+ }
+
+ }
+ motors.setSpeed (normalSpeed);
+ wait (0.5);
+
+ }
}
void measureSensors () {