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@3:711ac018d3ec, 2015-06-03 (annotated)

Committer:
crmackey
Date:
Wed Jun 03 12:55:00 2015 +0000
Revision:
3:711ac018d3ec
Parent:
2:91d60975d3e1
Child:
4:a0baf6752548
Sends sensor values to BT

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 0:f4302186f4cb 31 int s1, s2, s3, s4, s5;
crmackey 0:f4302186f4cb 32 Timer TurnTime;
crmackey 3:711ac018d3ec 33 Timer LapTime;
crmackey 0:f4302186f4cb 34
crmackey 0:f4302186f4cb 35 void RightTurn() {
crmackey 0:f4302186f4cb 36
crmackey 0:f4302186f4cb 37 m3pi.stop();
crmackey 0:f4302186f4cb 38 TurnTime.reset();
crmackey 0:f4302186f4cb 39 TurnTime.start();
crmackey 0:f4302186f4cb 40
crmackey 0:f4302186f4cb 41 while (1) {
crmackey 0:f4302186f4cb 42
crmackey 0:f4302186f4cb 43 m3pi.right_motor(0.45);
crmackey 0:f4302186f4cb 44 m3pi.left_motor(-0.1);
crmackey 0:f4302186f4cb 45
crmackey 0:f4302186f4cb 46 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 47
crmackey 0:f4302186f4cb 48 return;
crmackey 0:f4302186f4cb 49
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 void LeftTurn() {
crmackey 0:f4302186f4cb 58
crmackey 0:f4302186f4cb 59 m3pi.stop();
crmackey 0:f4302186f4cb 60 TurnTime.reset();
crmackey 0:f4302186f4cb 61 TurnTime.start();
crmackey 0:f4302186f4cb 62
crmackey 0:f4302186f4cb 63 while (1) {
crmackey 0:f4302186f4cb 64
crmackey 0:f4302186f4cb 65 m3pi.left_motor(0.45);
crmackey 0:f4302186f4cb 66 m3pi.right_motor(-0.1);
crmackey 0:f4302186f4cb 67
crmackey 0:f4302186f4cb 68 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 69
crmackey 0:f4302186f4cb 70 return;
crmackey 0:f4302186f4cb 71
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 int main() {
crmackey 0:f4302186f4cb 79
crmackey 0:f4302186f4cb 80 btbee.reset();
crmackey 0:f4302186f4cb 81 m3pi_pb.mode(PullUp);
crmackey 0:f4302186f4cb 82
crmackey 0:f4302186f4cb 83 m3pi.printf("Wait 4");
crmackey 0:f4302186f4cb 84 m3pi.locate(0,1);
crmackey 0:f4302186f4cb 85 m3pi.printf("PC");
crmackey 0:f4302186f4cb 86
crmackey 0:f4302186f4cb 87 while(m3pi_pb) { // Loop that runs until user presses button after bluetooth is connected.
crmackey 0:f4302186f4cb 88 m3pi_led[0]=!m3pi_led[0];
crmackey 0:f4302186f4cb 89 wait(3);
crmackey 0:f4302186f4cb 90 btbee.printf("\n");
crmackey 0:f4302186f4cb 91 btbee.printf("PC connected. Press the button \n");
crmackey 0:f4302186f4cb 92 }
crmackey 0:f4302186f4cb 93
crmackey 0:f4302186f4cb 94 m3pi.cls();
crmackey 0:f4302186f4cb 95
crmackey 3:711ac018d3ec 96 // reads battery voltage to screen on start up
crmackey 0:f4302186f4cb 97 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:f4302186f4cb 98 m3pi.print(Bat,7);
crmackey 0:f4302186f4cb 99 wait(0.75);
crmackey 0:f4302186f4cb 100 m3pi.cls();
crmackey 0:f4302186f4cb 101 float batteryvoltage = m3pi.battery();
crmackey 0:f4302186f4cb 102 char* str = new char[30];
crmackey 0:f4302186f4cb 103 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:f4302186f4cb 104 m3pi.print(str,6);
crmackey 0:f4302186f4cb 105 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:f4302186f4cb 106 wait(0.75);
crmackey 0:f4302186f4cb 107 m3pi.cls();
crmackey 0:f4302186f4cb 108
crmackey 0:f4302186f4cb 109 if (batteryvoltage < 4.2) // exits program if voltage is less than 4.2 V
crmackey 0:f4302186f4cb 110 {
crmackey 0:f4302186f4cb 111 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:f4302186f4cb 112 m3pi.print(low,7);
crmackey 0:f4302186f4cb 113 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:f4302186f4cb 114 m3pi.playtune(ExitSound,11);
crmackey 0:f4302186f4cb 115 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:f4302186f4cb 116 exit(1);
crmackey 0:f4302186f4cb 117 }
crmackey 0:f4302186f4cb 118
crmackey 0:f4302186f4cb 119 btbee.printf("Now calibrating \n"); // calibrating robot on the line.
crmackey 0:f4302186f4cb 120
crmackey 0:f4302186f4cb 121 m3pi.sensor_auto_calibrate();
crmackey 0:f4302186f4cb 122
crmackey 0:f4302186f4cb 123 btbee.printf("Finished calibrating \n");
crmackey 0:f4302186f4cb 124
crmackey 0:f4302186f4cb 125
crmackey 0:f4302186f4cb 126 btbee.printf("\n");
crmackey 0:f4302186f4cb 127 btbee.printf("Now starting \n");
crmackey 0:f4302186f4cb 128
crmackey 3:711ac018d3ec 129 LapTime.start();
crmackey 3:711ac018d3ec 130
crmackey 0:f4302186f4cb 131
crmackey 0:f4302186f4cb 132 while (1) {
crmackey 0:f4302186f4cb 133
crmackey 0:f4302186f4cb 134
crmackey 0:f4302186f4cb 135 // Get the position of the line.
crmackey 0:f4302186f4cb 136 current_pos_of_line = m3pi.line_position();
crmackey 0:f4302186f4cb 137 proportional = current_pos_of_line;
crmackey 0:f4302186f4cb 138
crmackey 0:f4302186f4cb 139 // Compute the derivative
crmackey 0:f4302186f4cb 140 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:f4302186f4cb 141
crmackey 0:f4302186f4cb 142 // Compute the integral
crmackey 0:f4302186f4cb 143 integral += proportional;
crmackey 0:f4302186f4cb 144
crmackey 0:f4302186f4cb 145 // Remember the last position.
crmackey 0:f4302186f4cb 146 previous_pos_of_line = current_pos_of_line;
crmackey 0:f4302186f4cb 147
crmackey 0:f4302186f4cb 148 // Compute the power
crmackey 0:f4302186f4cb 149 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:f4302186f4cb 150
crmackey 0:f4302186f4cb 151 // Compute new speeds
crmackey 0:f4302186f4cb 152 right = speed+power;
crmackey 0:f4302186f4cb 153 left = speed-power;
crmackey 0:f4302186f4cb 154
crmackey 0:f4302186f4cb 155 // limit checks
crmackey 0:f4302186f4cb 156 if (right < MIN)
crmackey 0:f4302186f4cb 157 right = MIN;
crmackey 0:f4302186f4cb 158 else if (right > MAX)
crmackey 0:f4302186f4cb 159 right = MAX;
crmackey 0:f4302186f4cb 160
crmackey 0:f4302186f4cb 161 if (left < MIN)
crmackey 0:f4302186f4cb 162 left = MIN;
crmackey 0:f4302186f4cb 163 else if (left > MAX)
crmackey 0:f4302186f4cb 164 left = MAX;
crmackey 0:f4302186f4cb 165
crmackey 0:f4302186f4cb 166 // set speed
crmackey 0:f4302186f4cb 167 m3pi.left_motor(left);
crmackey 0:f4302186f4cb 168 m3pi.right_motor(right);
crmackey 0:f4302186f4cb 169
crmackey 0:f4302186f4cb 170
crmackey 0:f4302186f4cb 171 m3pi.calibrated_sensor(LapTest);
crmackey 0:f4302186f4cb 172
crmackey 0:f4302186f4cb 173 s1 = LapTest[0];
crmackey 0:f4302186f4cb 174 s2 = LapTest[1];
crmackey 0:f4302186f4cb 175 s3 = LapTest[2];
crmackey 0:f4302186f4cb 176 s4 = LapTest[3];
crmackey 0:f4302186f4cb 177 s5 = LapTest[4];
crmackey 0:f4302186f4cb 178
crmackey 3:711ac018d3ec 179 if(LapTime.read() > 1.0) {
crmackey 3:711ac018d3ec 180
crmackey 3:711ac018d3ec 181 btbee.printf("Sensor 1: %f\n", s1);
crmackey 3:711ac018d3ec 182 btbee.printf("Sensor 2: %f\n", s2);
crmackey 3:711ac018d3ec 183 btbee.printf("Sensor 3: %f\n", s3);
crmackey 3:711ac018d3ec 184 btbee.printf("Sensor 4: %f\n", s4);
crmackey 3:711ac018d3ec 185 btbee.printf("Sensor 5: %f\n", s5);
crmackey 3:711ac018d3ec 186 btbee.printf("\n");
crmackey 3:711ac018d3ec 187 LapTime.reset();
crmackey 3:711ac018d3ec 188 LapTime.start();
crmackey 3:711ac018d3ec 189
crmackey 3:711ac018d3ec 190 }
crmackey 0:f4302186f4cb 191
crmackey 3:711ac018d3ec 192
crmackey 3:711ac018d3ec 193 if (s1 > 700 and s2 > 700 and s3 > 700 and s4 > 700 and s5 > 700) {
crmackey 0:f4302186f4cb 194
crmackey 2:91d60975d3e1 195 LeftTurn();
crmackey 0:f4302186f4cb 196
crmackey 0:f4302186f4cb 197 }
crmackey 0:f4302186f4cb 198
crmackey 0:f4302186f4cb 199
crmackey 0:f4302186f4cb 200 }
crmackey 0:f4302186f4cb 201
crmackey 0:f4302186f4cb 202 }
crmackey 0:f4302186f4cb 203