Coursework Buggy / Mbed 2 deprecated 161015

speed code

Dependencies:   mbed

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

Committer:
benparkes
Date:
Fri Oct 16 12:39:12 2015 +0000
Revision:
0:e79700919e2e
Child:
1:f4e3365155e1
16/10/15;

Who changed what in which revision?

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