Coursework Buggy / Mbed 2 deprecated extension2

extension 2

Dependencies:   mbed

main.cpp@0:d1ef26d38f28, 2015-10-16 (annotated)

Committer:
benparkes
Date:
Fri Oct 16 14:45:44 2015 +0000
Revision:
0:d1ef26d38f28
extension 2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benparkes 0:d1ef26d38f28 1 #include "mbed.h"
benparkes 0:d1ef26d38f28 2
benparkes 0:d1ef26d38f28 3 #define wheelc 0.1759292
benparkes 0:d1ef26d38f28 4 #define pi 3.141592654
benparkes 0:d1ef26d38f28 5 #define degreel 0.021988888
benparkes 0:d1ef26d38f28 6 //Status LED
benparkes 0:d1ef26d38f28 7 DigitalOut led(LED1);
benparkes 0:d1ef26d38f28 8
benparkes 0:d1ef26d38f28 9 //Motor PWM (speed)
benparkes 0:d1ef26d38f28 10 PwmOut PWMA(PA_8);
benparkes 0:d1ef26d38f28 11 PwmOut PWMB(PB_4);
benparkes 0:d1ef26d38f28 12
benparkes 0:d1ef26d38f28 13 //Motor Direction
benparkes 0:d1ef26d38f28 14 DigitalOut DIRA(PA_9);
benparkes 0:d1ef26d38f28 15 DigitalOut DIRB(PB_10);
benparkes 0:d1ef26d38f28 16
benparkes 0:d1ef26d38f28 17 //Hall-Effect Sensor Inputs
benparkes 0:d1ef26d38f28 18 InterruptIn HEA1(PB_2);
benparkes 0:d1ef26d38f28 19 DigitalIn HEA2(PB_1);
benparkes 0:d1ef26d38f28 20 InterruptIn HEB1(PB_15);
benparkes 0:d1ef26d38f28 21 DigitalIn HEB2(PB_14);
benparkes 0:d1ef26d38f28 22
benparkes 0:d1ef26d38f28 23 //On board switch
benparkes 0:d1ef26d38f28 24 DigitalIn SW1(USER_BUTTON);
benparkes 0:d1ef26d38f28 25
benparkes 0:d1ef26d38f28 26 //Use the serial object so we can use higher speeds
benparkes 0:d1ef26d38f28 27 Serial terminal(USBTX, USBRX);
benparkes 0:d1ef26d38f28 28
benparkes 0:d1ef26d38f28 29 //Timer used for measuring speeds
benparkes 0:d1ef26d38f28 30 Timer timer;
benparkes 0:d1ef26d38f28 31
benparkes 0:d1ef26d38f28 32 //Enumerated types
benparkes 0:d1ef26d38f28 33 enum DIRECTION {FORWARD=0, REVERSE};
benparkes 0:d1ef26d38f28 34 enum PULSE {NOPULSE=0, PULSE};
benparkes 0:d1ef26d38f28 35 enum SWITCHSTATE {PRESSED=0, RELEASED};
benparkes 0:d1ef26d38f28 36
benparkes 0:d1ef26d38f28 37 //Debug GPIO
benparkes 0:d1ef26d38f28 38 DigitalOut probe(D10);
benparkes 0:d1ef26d38f28 39
benparkes 0:d1ef26d38f28 40 //Duty cycles
benparkes 0:d1ef26d38f28 41 float dutyA = 0.100f; //100%
benparkes 0:d1ef26d38f28 42 float dutyB = 0.100f; //100%
benparkes 0:d1ef26d38f28 43
benparkes 0:d1ef26d38f28 44 //distance measurement
benparkes 0:d1ef26d38f28 45 float distanceA ;
benparkes 0:d1ef26d38f28 46 float distanceB ;
benparkes 0:d1ef26d38f28 47 float speedA ;
benparkes 0:d1ef26d38f28 48 float speedB ;
benparkes 0:d1ef26d38f28 49 float pretimerA;
benparkes 0:d1ef26d38f28 50 float afttimerA;
benparkes 0:d1ef26d38f28 51 float pretimerB;
benparkes 0:d1ef26d38f28 52 float afttimerB;
benparkes 0:d1ef26d38f28 53 float wheel_spacing = 0.128;
benparkes 0:d1ef26d38f28 54 float lastinterupttimeA = 0.000f;
benparkes 0:d1ef26d38f28 55 float lastinterupttimeB = 0.000f;
benparkes 0:d1ef26d38f28 56
benparkes 0:d1ef26d38f28 57 //Completed Loop
benparkes 0:d1ef26d38f28 58 int loop=0;
benparkes 0:d1ef26d38f28 59
benparkes 0:d1ef26d38f28 60 int turn();
benparkes 0:d1ef26d38f28 61 void ResetDistanceTimeSpeed()
benparkes 0:d1ef26d38f28 62 {
benparkes 0:d1ef26d38f28 63 distanceA=0;
benparkes 0:d1ef26d38f28 64 distanceB=0;
benparkes 0:d1ef26d38f28 65 speedA=0;
benparkes 0:d1ef26d38f28 66 speedB=0;
benparkes 0:d1ef26d38f28 67 pretimerA=0;
benparkes 0:d1ef26d38f28 68 pretimerB=0;
benparkes 0:d1ef26d38f28 69 afttimerA=0;
benparkes 0:d1ef26d38f28 70 afttimerB=0;
benparkes 0:d1ef26d38f28 71 timer.reset();
benparkes 0:d1ef26d38f28 72 timer.start();
benparkes 0:d1ef26d38f28 73 }
benparkes 0:d1ef26d38f28 74
benparkes 0:d1ef26d38f28 75 void stopmotors()
benparkes 0:d1ef26d38f28 76 {
benparkes 0:d1ef26d38f28 77 PWMA.write(0.0f); //0% duty cycle
benparkes 0:d1ef26d38f28 78 PWMB.write(0.0f); //0% duty cycle
benparkes 0:d1ef26d38f28 79 }
benparkes 0:d1ef26d38f28 80
benparkes 0:d1ef26d38f28 81 int forward(float distance, float speed)
benparkes 0:d1ef26d38f28 82 {
benparkes 0:d1ef26d38f28 83 //add forward to input with switch for scalability
benparkes 0:d1ef26d38f28 84 // Set motor direction forward
benparkes 0:d1ef26d38f28 85 DIRA = FORWARD;
benparkes 0:d1ef26d38f28 86 DIRB = FORWARD;
benparkes 0:d1ef26d38f28 87
benparkes 0:d1ef26d38f28 88 //reset distance
benparkes 0:d1ef26d38f28 89 ResetDistanceTimeSpeed();
benparkes 0:d1ef26d38f28 90
benparkes 0:d1ef26d38f28 91 // Set motor speed to input speed
benparkes 0:d1ef26d38f28 92 PWMA.write(0.1f); //Set duty cycle (%)
benparkes 0:d1ef26d38f28 93 PWMB.write(0.1f); //Set duty cycle (%)
benparkes 0:d1ef26d38f28 94
benparkes 0:d1ef26d38f28 95
benparkes 0:d1ef26d38f28 96 //wait for run to complete
benparkes 0:d1ef26d38f28 97 while (((distanceA+distanceB)/2) < distance) {
benparkes 0:d1ef26d38f28 98
benparkes 0:d1ef26d38f28 99 if (speedA<speed) {
benparkes 0:d1ef26d38f28 100
benparkes 0:d1ef26d38f28 101 dutyA += (float)0.0051;
benparkes 0:d1ef26d38f28 102 PWMA.write(dutyA);
benparkes 0:d1ef26d38f28 103 }
benparkes 0:d1ef26d38f28 104 if( speedA>speed) {
benparkes 0:d1ef26d38f28 105 dutyA -= (float)0.0051;
benparkes 0:d1ef26d38f28 106 PWMA.write(dutyA);
benparkes 0:d1ef26d38f28 107 }
benparkes 0:d1ef26d38f28 108
benparkes 0:d1ef26d38f28 109 if (speedB<speed) {
benparkes 0:d1ef26d38f28 110
benparkes 0:d1ef26d38f28 111 dutyB += (float)0.0051;
benparkes 0:d1ef26d38f28 112 PWMB.write(dutyB);
benparkes 0:d1ef26d38f28 113 }
benparkes 0:d1ef26d38f28 114 if( speedB>speed) {
benparkes 0:d1ef26d38f28 115 dutyB -= (float)0.0051;
benparkes 0:d1ef26d38f28 116 PWMB.write(dutyB);
benparkes 0:d1ef26d38f28 117 }
benparkes 0:d1ef26d38f28 118 wait(0.05);
benparkes 0:d1ef26d38f28 119 }
benparkes 0:d1ef26d38f28 120 return 1;
benparkes 0:d1ef26d38f28 121 }
benparkes 0:d1ef26d38f28 122
benparkes 0:d1ef26d38f28 123 int turn(float degrees, float speed, int direction) // (Degrees of Turn, Speed, (Anti/Clock)wise)
benparkes 0:d1ef26d38f28 124
benparkes 0:d1ef26d38f28 125 {
benparkes 0:d1ef26d38f28 126 // Calculate Turn Distance
benparkes 0:d1ef26d38f28 127 float distance=0;
benparkes 0:d1ef26d38f28 128 distance=((float)degreel*degrees);
benparkes 0:d1ef26d38f28 129 //Set Initial Motor Direction
benparkes 0:d1ef26d38f28 130 DIRA = FORWARD;
benparkes 0:d1ef26d38f28 131 DIRB = FORWARD;
benparkes 0:d1ef26d38f28 132
benparkes 0:d1ef26d38f28 133 // Set Motor Speed for Outside Wheel
benparkes 0:d1ef26d38f28 134 PWMA.write(0.0f); //Set duty cycle (%)
benparkes 0:d1ef26d38f28 135 PWMB.write(0.0f); //Set duty cycle (%)
benparkes 0:d1ef26d38f28 136
benparkes 0:d1ef26d38f28 137 // Test for Loop Completion to Enter Victory Spin
benparkes 0:d1ef26d38f28 138 if(direction==REVERSE) {
benparkes 0:d1ef26d38f28 139 DIRA = REVERSE;
benparkes 0:d1ef26d38f28 140 DIRB = FORWARD;
benparkes 0:d1ef26d38f28 141 PWMB.write(0.0f); //Set duty cycle (%)
benparkes 0:d1ef26d38f28 142 }
benparkes 0:d1ef26d38f28 143
benparkes 0:d1ef26d38f28 144
benparkes 0:d1ef26d38f28 145
benparkes 0:d1ef26d38f28 146 // Reset Distance Travelled
benparkes 0:d1ef26d38f28 147 ResetDistanceTimeSpeed();
benparkes 0:d1ef26d38f28 148 // Wait for Turn to Complete
benparkes 0:d1ef26d38f28 149 while (distanceA < distance) {
benparkes 0:d1ef26d38f28 150 if (speedA<speed) {
benparkes 0:d1ef26d38f28 151
benparkes 0:d1ef26d38f28 152 dutyA += (float)0.0051;
benparkes 0:d1ef26d38f28 153 PWMA.write(dutyA);
benparkes 0:d1ef26d38f28 154 }
benparkes 0:d1ef26d38f28 155 if( speedA>speed) {
benparkes 0:d1ef26d38f28 156 dutyA -= (float)0.0051;
benparkes 0:d1ef26d38f28 157 PWMA.write(dutyA);
benparkes 0:d1ef26d38f28 158 }
benparkes 0:d1ef26d38f28 159
benparkes 0:d1ef26d38f28 160 if(direction==REVERSE) {
benparkes 0:d1ef26d38f28 161 if (speedB<(speed/2)) {
benparkes 0:d1ef26d38f28 162
benparkes 0:d1ef26d38f28 163 dutyB += (float)0.0051;
benparkes 0:d1ef26d38f28 164 PWMB.write(dutyB);
benparkes 0:d1ef26d38f28 165 }
benparkes 0:d1ef26d38f28 166 if( speedB>(speed/2)) {
benparkes 0:d1ef26d38f28 167 dutyB -= (float)0.0051;
benparkes 0:d1ef26d38f28 168 PWMB.write(dutyB);
benparkes 0:d1ef26d38f28 169 }
benparkes 0:d1ef26d38f28 170 }
benparkes 0:d1ef26d38f28 171 wait(0.1);
benparkes 0:d1ef26d38f28 172 }
benparkes 0:d1ef26d38f28 173 return 1;
benparkes 0:d1ef26d38f28 174 }
benparkes 0:d1ef26d38f28 175
benparkes 0:d1ef26d38f28 176
benparkes 0:d1ef26d38f28 177
benparkes 0:d1ef26d38f28 178 void set_distanceA()
benparkes 0:d1ef26d38f28 179 {
benparkes 0:d1ef26d38f28 180 float time = 0;
benparkes 0:d1ef26d38f28 181 float interupttime = timer.read_us();
benparkes 0:d1ef26d38f28 182
benparkes 0:d1ef26d38f28 183
benparkes 0:d1ef26d38f28 184 if(interupttime-lastinterupttimeA > 5) {
benparkes 0:d1ef26d38f28 185
benparkes 0:d1ef26d38f28 186 afttimerA = timer.read(); //set latest timer to equal timer
benparkes 0:d1ef26d38f28 187 distanceA += (wheelc/6); //set distance travelled for this instruction i.e forward/turn etc
benparkes 0:d1ef26d38f28 188 time = afttimerA - pretimerA; //work out time taken for last 6th of a rotation
benparkes 0:d1ef26d38f28 189 speedA = 0.166667f/(time); //distance/time = average speed for last 6th rotation
benparkes 0:d1ef26d38f28 190 pretimerA = afttimerA; //update pretimer for next calculation of time
benparkes 0:d1ef26d38f28 191 terminal.printf("speedA %f : time %f\n\r", speedA, time);
benparkes 0:d1ef26d38f28 192 }
benparkes 0:d1ef26d38f28 193 lastinterupttimeA = interupttime;
benparkes 0:d1ef26d38f28 194 }
benparkes 0:d1ef26d38f28 195
benparkes 0:d1ef26d38f28 196 void set_distanceB()
benparkes 0:d1ef26d38f28 197 {
benparkes 0:d1ef26d38f28 198 float time = 0;
benparkes 0:d1ef26d38f28 199 float interupttime = timer.read_us();
benparkes 0:d1ef26d38f28 200
benparkes 0:d1ef26d38f28 201 if(interupttime-lastinterupttimeB > 5) {
benparkes 0:d1ef26d38f28 202 afttimerB = timer.read();
benparkes 0:d1ef26d38f28 203 distanceB += (wheelc/6);
benparkes 0:d1ef26d38f28 204 time = afttimerB - pretimerB;
benparkes 0:d1ef26d38f28 205 speedB = 0.166667f/time;
benparkes 0:d1ef26d38f28 206 pretimerB = afttimerB;
benparkes 0:d1ef26d38f28 207 terminal.printf("speedB %f : time %f\n\r", speedB, time);
benparkes 0:d1ef26d38f28 208 }
benparkes 0:d1ef26d38f28 209 lastinterupttimeB = interupttime;
benparkes 0:d1ef26d38f28 210 }
benparkes 0:d1ef26d38f28 211
benparkes 0:d1ef26d38f28 212 int main()
benparkes 0:d1ef26d38f28 213 {
benparkes 0:d1ef26d38f28 214 //Configure the terminal to high speed
benparkes 0:d1ef26d38f28 215 terminal.baud(115200);
benparkes 0:d1ef26d38f28 216
benparkes 0:d1ef26d38f28 217 HEA1.rise(set_distanceA);
benparkes 0:d1ef26d38f28 218 HEB1.rise(set_distanceB);
benparkes 0:d1ef26d38f28 219 //Set initial motor speed to stop
benparkes 0:d1ef26d38f28 220 stopmotors();
benparkes 0:d1ef26d38f28 221 PWMA.period_ms(10);
benparkes 0:d1ef26d38f28 222 PWMB.period_ms(10);
benparkes 0:d1ef26d38f28 223 while(1) {
benparkes 0:d1ef26d38f28 224 //wait for switch press
benparkes 0:d1ef26d38f28 225 while (SW1 == PRESSED) {
benparkes 0:d1ef26d38f28 226 wait(0.01);
benparkes 0:d1ef26d38f28 227 while(SW1 == RELEASED) {
benparkes 0:d1ef26d38f28 228
benparkes 0:d1ef26d38f28 229 //navigate course
benparkes 0:d1ef26d38f28 230 for (int i = 0; i<2; i++) {
benparkes 0:d1ef26d38f28 231 forward(12,10);
benparkes 0:d1ef26d38f28 232 //terminal.printf("turn\n\r");
benparkes 0:d1ef26d38f28 233 turn(90,10,0);
benparkes 0:d1ef26d38f28 234 //terminal.printf("forward\n\r");
benparkes 0:d1ef26d38f28 235 forward(7,10);
benparkes 0:d1ef26d38f28 236 //terminal.printf("turn\n\r");
benparkes 0:d1ef26d38f28 237 turn(90,10,0);
benparkes 0:d1ef26d38f28 238 }
benparkes 0:d1ef26d38f28 239
benparkes 0:d1ef26d38f28 240 stopmotors();
benparkes 0:d1ef26d38f28 241
benparkes 0:d1ef26d38f28 242 wait(0.5);
benparkes 0:d1ef26d38f28 243
benparkes 0:d1ef26d38f28 244 //victory spin
benparkes 0:d1ef26d38f28 245 turn(180,40,1);
benparkes 0:d1ef26d38f28 246
benparkes 0:d1ef26d38f28 247 stopmotors();
benparkes 0:d1ef26d38f28 248 break;
benparkes 0:d1ef26d38f28 249 }
benparkes 0:d1ef26d38f28 250 }
benparkes 0:d1ef26d38f28 251 }
benparkes 0:d1ef26d38f28 252
benparkes 0:d1ef26d38f28 253 }
benparkes 0:d1ef26d38f28 254