Coursework Buggy / Mbed 2 deprecated Enhancement2

16/10/15

Dependencies:   mbed

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

Committer:
ABentley
Date:
Fri Oct 16 14:07:14 2015 +0000
Revision:
0:d2ec2e0bf935
As of 16/10/15

Who changed what in which revision?

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