MEGACARAZ / Mbed 2 deprecated Turning

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   btbee m3pi_ng mbed

main.cpp@4:a0baf6752548, 2015-06-03 (annotated)

Committer:
crmackey
Date:
Wed Jun 03 13:37:01 2015 +0000
Revision:
4:a0baf6752548
Parent:
3:711ac018d3ec
Child:
5:0c2da538ada2
Follows from 2 to 4, then stops at 4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
crmackey 0:f4302186f4cb 1 #include "mbed.h"
crmackey 0:f4302186f4cb 2 #include "m3pi_ng.h"
crmackey 0:f4302186f4cb 3 #include "time.h"
crmackey 0:f4302186f4cb 4 #include "btbee.h"
crmackey 0:f4302186f4cb 5
crmackey 0:f4302186f4cb 6 m3pi m3pi;
crmackey 0:f4302186f4cb 7 btbee btbee;
crmackey 0:f4302186f4cb 8 DigitalIn m3pi_pb(p21);
crmackey 0:f4302186f4cb 9 DigitalIn m3pi_IN[] = {(p12)};
crmackey 0:f4302186f4cb 10 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
crmackey 0:f4302186f4cb 11 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
crmackey 0:f4302186f4cb 12
crmackey 0:f4302186f4cb 13 // Minimum and maximum motor speeds
crmackey 0:f4302186f4cb 14 #define MAX 0.5
crmackey 0:f4302186f4cb 15 #define MIN 0
crmackey 0:f4302186f4cb 16
crmackey 0:f4302186f4cb 17 // PID terms
crmackey 0:f4302186f4cb 18 #define P_TERM 1.5
crmackey 0:f4302186f4cb 19 #define I_TERM 0
crmackey 0:f4302186f4cb 20 #define D_TERM 25
crmackey 0:f4302186f4cb 21
crmackey 0:f4302186f4cb 22
crmackey 0:f4302186f4cb 23 float right; // initializes variables
crmackey 0:f4302186f4cb 24 float left;
crmackey 0:f4302186f4cb 25 float current_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 26 float previous_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 27 float derivative,proportional,integral = 0;
crmackey 0:f4302186f4cb 28 float power;
crmackey 0:f4302186f4cb 29 float speed = MAX;
crmackey 0:f4302186f4cb 30 int LapTest[5];
crmackey 4:a0baf6752548 31 float s1, s2, s3, s4, s5;
crmackey 0:f4302186f4cb 32 Timer TurnTime;
crmackey 3:711ac018d3ec 33 Timer LapTime;
crmackey 4:a0baf6752548 34 int counter = 0;
crmackey 0:f4302186f4cb 35
crmackey 0:f4302186f4cb 36 void RightTurn() {
crmackey 0:f4302186f4cb 37
crmackey 0:f4302186f4cb 38 m3pi.stop();
crmackey 0:f4302186f4cb 39 TurnTime.reset();
crmackey 0:f4302186f4cb 40 TurnTime.start();
crmackey 0:f4302186f4cb 41
crmackey 0:f4302186f4cb 42 while (1) {
crmackey 0:f4302186f4cb 43
crmackey 0:f4302186f4cb 44 m3pi.right_motor(0.45);
crmackey 0:f4302186f4cb 45 m3pi.left_motor(-0.1);
crmackey 0:f4302186f4cb 46
crmackey 0:f4302186f4cb 47 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 48
crmackey 0:f4302186f4cb 49 return;
crmackey 0:f4302186f4cb 50
crmackey 0:f4302186f4cb 51 }
crmackey 0:f4302186f4cb 52
crmackey 0:f4302186f4cb 53 }
crmackey 0:f4302186f4cb 54
crmackey 0:f4302186f4cb 55 }
crmackey 0:f4302186f4cb 56
crmackey 0:f4302186f4cb 57
crmackey 0:f4302186f4cb 58 void LeftTurn() {
crmackey 0:f4302186f4cb 59
crmackey 0:f4302186f4cb 60 m3pi.stop();
crmackey 0:f4302186f4cb 61 TurnTime.reset();
crmackey 0:f4302186f4cb 62 TurnTime.start();
crmackey 0:f4302186f4cb 63
crmackey 0:f4302186f4cb 64 while (1) {
crmackey 0:f4302186f4cb 65
crmackey 0:f4302186f4cb 66 m3pi.left_motor(0.45);
crmackey 0:f4302186f4cb 67 m3pi.right_motor(-0.1);
crmackey 0:f4302186f4cb 68
crmackey 0:f4302186f4cb 69 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 70
crmackey 0:f4302186f4cb 71 return;
crmackey 0:f4302186f4cb 72
crmackey 0:f4302186f4cb 73 }
crmackey 0:f4302186f4cb 74
crmackey 0:f4302186f4cb 75 }
crmackey 0:f4302186f4cb 76
crmackey 0:f4302186f4cb 77 }
crmackey 0:f4302186f4cb 78
crmackey 0:f4302186f4cb 79 int main() {
crmackey 0:f4302186f4cb 80
crmackey 0:f4302186f4cb 81 btbee.reset();
crmackey 0:f4302186f4cb 82 m3pi_pb.mode(PullUp);
crmackey 0:f4302186f4cb 83
crmackey 0:f4302186f4cb 84 m3pi.printf("Wait 4");
crmackey 0:f4302186f4cb 85 m3pi.locate(0,1);
crmackey 0:f4302186f4cb 86 m3pi.printf("PC");
crmackey 0:f4302186f4cb 87
crmackey 0:f4302186f4cb 88 while(m3pi_pb) { // Loop that runs until user presses button after bluetooth is connected.
crmackey 0:f4302186f4cb 89 m3pi_led[0]=!m3pi_led[0];
crmackey 0:f4302186f4cb 90 wait(3);
crmackey 0:f4302186f4cb 91 btbee.printf("\n");
crmackey 0:f4302186f4cb 92 btbee.printf("PC connected. Press the button \n");
crmackey 0:f4302186f4cb 93 }
crmackey 0:f4302186f4cb 94
crmackey 0:f4302186f4cb 95 m3pi.cls();
crmackey 0:f4302186f4cb 96
crmackey 3:711ac018d3ec 97 // reads battery voltage to screen on start up
crmackey 0:f4302186f4cb 98 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:f4302186f4cb 99 m3pi.print(Bat,7);
crmackey 0:f4302186f4cb 100 wait(0.75);
crmackey 0:f4302186f4cb 101 m3pi.cls();
crmackey 0:f4302186f4cb 102 float batteryvoltage = m3pi.battery();
crmackey 0:f4302186f4cb 103 char* str = new char[30];
crmackey 0:f4302186f4cb 104 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:f4302186f4cb 105 m3pi.print(str,6);
crmackey 0:f4302186f4cb 106 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:f4302186f4cb 107 wait(0.75);
crmackey 0:f4302186f4cb 108 m3pi.cls();
crmackey 0:f4302186f4cb 109
crmackey 0:f4302186f4cb 110 if (batteryvoltage < 4.2) // exits program if voltage is less than 4.2 V
crmackey 0:f4302186f4cb 111 {
crmackey 0:f4302186f4cb 112 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:f4302186f4cb 113 m3pi.print(low,7);
crmackey 0:f4302186f4cb 114 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:f4302186f4cb 115 m3pi.playtune(ExitSound,11);
crmackey 0:f4302186f4cb 116 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:f4302186f4cb 117 exit(1);
crmackey 0:f4302186f4cb 118 }
crmackey 0:f4302186f4cb 119
crmackey 0:f4302186f4cb 120 btbee.printf("Now calibrating \n"); // calibrating robot on the line.
crmackey 0:f4302186f4cb 121
crmackey 0:f4302186f4cb 122 m3pi.sensor_auto_calibrate();
crmackey 0:f4302186f4cb 123
crmackey 0:f4302186f4cb 124 btbee.printf("Finished calibrating \n");
crmackey 0:f4302186f4cb 125
crmackey 0:f4302186f4cb 126
crmackey 0:f4302186f4cb 127 btbee.printf("\n");
crmackey 0:f4302186f4cb 128 btbee.printf("Now starting \n");
crmackey 0:f4302186f4cb 129
crmackey 3:711ac018d3ec 130 LapTime.start();
crmackey 3:711ac018d3ec 131
crmackey 0:f4302186f4cb 132
crmackey 0:f4302186f4cb 133 while (1) {
crmackey 0:f4302186f4cb 134
crmackey 0:f4302186f4cb 135
crmackey 0:f4302186f4cb 136 // Get the position of the line.
crmackey 0:f4302186f4cb 137 current_pos_of_line = m3pi.line_position();
crmackey 0:f4302186f4cb 138 proportional = current_pos_of_line;
crmackey 0:f4302186f4cb 139
crmackey 0:f4302186f4cb 140 // Compute the derivative
crmackey 0:f4302186f4cb 141 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:f4302186f4cb 142
crmackey 0:f4302186f4cb 143 // Compute the integral
crmackey 0:f4302186f4cb 144 integral += proportional;
crmackey 0:f4302186f4cb 145
crmackey 0:f4302186f4cb 146 // Remember the last position.
crmackey 0:f4302186f4cb 147 previous_pos_of_line = current_pos_of_line;
crmackey 0:f4302186f4cb 148
crmackey 0:f4302186f4cb 149 // Compute the power
crmackey 0:f4302186f4cb 150 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:f4302186f4cb 151
crmackey 0:f4302186f4cb 152 // Compute new speeds
crmackey 0:f4302186f4cb 153 right = speed+power;
crmackey 0:f4302186f4cb 154 left = speed-power;
crmackey 0:f4302186f4cb 155
crmackey 0:f4302186f4cb 156 // limit checks
crmackey 0:f4302186f4cb 157 if (right < MIN)
crmackey 0:f4302186f4cb 158 right = MIN;
crmackey 0:f4302186f4cb 159 else if (right > MAX)
crmackey 0:f4302186f4cb 160 right = MAX;
crmackey 0:f4302186f4cb 161
crmackey 0:f4302186f4cb 162 if (left < MIN)
crmackey 0:f4302186f4cb 163 left = MIN;
crmackey 0:f4302186f4cb 164 else if (left > MAX)
crmackey 0:f4302186f4cb 165 left = MAX;
crmackey 0:f4302186f4cb 166
crmackey 0:f4302186f4cb 167 // set speed
crmackey 0:f4302186f4cb 168 m3pi.left_motor(left);
crmackey 0:f4302186f4cb 169 m3pi.right_motor(right);
crmackey 0:f4302186f4cb 170
crmackey 0:f4302186f4cb 171
crmackey 0:f4302186f4cb 172 m3pi.calibrated_sensor(LapTest);
crmackey 0:f4302186f4cb 173
crmackey 0:f4302186f4cb 174 s1 = LapTest[0];
crmackey 0:f4302186f4cb 175 s2 = LapTest[1];
crmackey 0:f4302186f4cb 176 s3 = LapTest[2];
crmackey 0:f4302186f4cb 177 s4 = LapTest[3];
crmackey 0:f4302186f4cb 178 s5 = LapTest[4];
crmackey 0:f4302186f4cb 179
crmackey 4:a0baf6752548 180 //if(LapTime.read() > 1.0) {
crmackey 4:a0baf6752548 181 //
crmackey 4:a0baf6752548 182 // btbee.printf("Sensor 1: %f\n", s1);
crmackey 4:a0baf6752548 183 // btbee.printf("Sensor 2: %f\n", s2);
crmackey 4:a0baf6752548 184 // btbee.printf("Sensor 3: %f\n", s3);
crmackey 4:a0baf6752548 185 // btbee.printf("Sensor 4: %f\n", s4);
crmackey 4:a0baf6752548 186 // btbee.printf("Sensor 5: %f\n", s5);
crmackey 4:a0baf6752548 187 // btbee.printf("\n");
crmackey 4:a0baf6752548 188 // LapTime.reset();
crmackey 4:a0baf6752548 189 // LapTime.start();
crmackey 4:a0baf6752548 190 //
crmackey 4:a0baf6752548 191 // }
crmackey 4:a0baf6752548 192
crmackey 4:a0baf6752548 193
crmackey 4:a0baf6752548 194
crmackey 4:a0baf6752548 195 if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500 and LapTime.read() > 0.1) {
crmackey 3:711ac018d3ec 196
crmackey 4:a0baf6752548 197 if( counter == 2) {
crmackey 4:a0baf6752548 198
crmackey 4:a0baf6752548 199 m3pi.stop();
crmackey 4:a0baf6752548 200 exit(1);
crmackey 4:a0baf6752548 201
crmackey 4:a0baf6752548 202 }
crmackey 4:a0baf6752548 203
crmackey 4:a0baf6752548 204 m3pi.stop();
crmackey 4:a0baf6752548 205 btbee.printf("Detected full cross");
crmackey 4:a0baf6752548 206 LeftTurn();
crmackey 4:a0baf6752548 207 btbee.printf("\n");
crmackey 4:a0baf6752548 208 LapTime.reset();
crmackey 4:a0baf6752548 209 LapTime.start();
crmackey 4:a0baf6752548 210 counter++;
crmackey 4:a0baf6752548 211
crmackey 4:a0baf6752548 212 }
crmackey 4:a0baf6752548 213
crmackey 4:a0baf6752548 214 else if( s5 > 700 and s4 > 700 and LapTime.read() > 0.1) {
crmackey 4:a0baf6752548 215
crmackey 4:a0baf6752548 216 m3pi.stop();
crmackey 4:a0baf6752548 217 btbee.printf("Detected right side half cross\n");
crmackey 4:a0baf6752548 218 RightTurn();
crmackey 3:711ac018d3ec 219 btbee.printf("\n");
crmackey 3:711ac018d3ec 220 LapTime.reset();
crmackey 3:711ac018d3ec 221 LapTime.start();
crmackey 3:711ac018d3ec 222
crmackey 3:711ac018d3ec 223 }
crmackey 4:a0baf6752548 224
crmackey 4:a0baf6752548 225 else if( s1 > 700 and s2 > 700 and LapTime.read() > 0.1) {
crmackey 4:a0baf6752548 226
crmackey 4:a0baf6752548 227 m3pi.stop();
crmackey 4:a0baf6752548 228 btbee.printf("Detected left side half cross\n");
crmackey 2:91d60975d3e1 229 LeftTurn();
crmackey 4:a0baf6752548 230 btbee.printf("\n");
crmackey 4:a0baf6752548 231 LapTime.reset();
crmackey 4:a0baf6752548 232 LapTime.start();
crmackey 0:f4302186f4cb 233
crmackey 0:f4302186f4cb 234 }
crmackey 0:f4302186f4cb 235
crmackey 0:f4302186f4cb 236
crmackey 0:f4302186f4cb 237 }
crmackey 0:f4302186f4cb 238
crmackey 0:f4302186f4cb 239 }
crmackey 0:f4302186f4cb 240