Coursework Buggy / Mbed 2 deprecated Coursework_Enhancement_1

Ready to be uploaded

Dependencies:   mbed

main.cpp@0:735ec5eb58d0, 2015-10-15 (annotated)

Committer:
ABentley
Date:
Thu Oct 15 13:43:39 2015 +0000
Revision:
0:735ec5eb58d0
Enhancement 1 complete. Ready to draft;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ABentley 0:735ec5eb58d0 1 #include "mbed.h"
ABentley 0:735ec5eb58d0 2
ABentley 0:735ec5eb58d0 3 #define wheelc 0.1759292 // Circumference of Wheel
ABentley 0:735ec5eb58d0 4 #define pi 3.141592654
ABentley 0:735ec5eb58d0 5 #define degreel 0.021988888 // Distance Travelled in 1 Degree
ABentley 0:735ec5eb58d0 6
ABentley 0:735ec5eb58d0 7
ABentley 0:735ec5eb58d0 8 // Status LED
ABentley 0:735ec5eb58d0 9 DigitalOut led(LED1);
ABentley 0:735ec5eb58d0 10
ABentley 0:735ec5eb58d0 11 // Motor PWM (speed)
ABentley 0:735ec5eb58d0 12 PwmOut PWMA(PA_8);
ABentley 0:735ec5eb58d0 13 PwmOut PWMB(PB_4);
ABentley 0:735ec5eb58d0 14
ABentley 0:735ec5eb58d0 15 // Motor Direction
ABentley 0:735ec5eb58d0 16 DigitalOut DIRA(PA_9);
ABentley 0:735ec5eb58d0 17 DigitalOut DIRB(PB_10);
ABentley 0:735ec5eb58d0 18
ABentley 0:735ec5eb58d0 19 // Hall-Effect Sensor Inputs
ABentley 0:735ec5eb58d0 20 InterruptIn HEA1(PB_2);
ABentley 0:735ec5eb58d0 21 DigitalIn HEA2(PB_1);
ABentley 0:735ec5eb58d0 22 InterruptIn HEB1(PB_15);
ABentley 0:735ec5eb58d0 23 DigitalIn HEB2(PB_14);
ABentley 0:735ec5eb58d0 24
ABentley 0:735ec5eb58d0 25 // On board Switch
ABentley 0:735ec5eb58d0 26 DigitalIn SW1(USER_BUTTON);
ABentley 0:735ec5eb58d0 27
ABentley 0:735ec5eb58d0 28 // Use the Serial Object so we can use Higher Speeds
ABentley 0:735ec5eb58d0 29 Serial terminal(USBTX, USBRX);
ABentley 0:735ec5eb58d0 30
ABentley 0:735ec5eb58d0 31 // Timer Used for Measuring Speeds
ABentley 0:735ec5eb58d0 32 Timer timer;
ABentley 0:735ec5eb58d0 33
ABentley 0:735ec5eb58d0 34 // Enumerated Types
ABentley 0:735ec5eb58d0 35 enum DIRECTION {FORWARD=0, REVERSE};
ABentley 0:735ec5eb58d0 36 enum PULSE {NOPULSE=0, PULSE};
ABentley 0:735ec5eb58d0 37 enum SWITCHSTATE {PRESSED=0, RELEASED};
ABentley 0:735ec5eb58d0 38
ABentley 0:735ec5eb58d0 39 // Debug GPIO
ABentley 0:735ec5eb58d0 40 DigitalOut probe(D10);
ABentley 0:735ec5eb58d0 41
ABentley 0:735ec5eb58d0 42 // Duty Cycles
ABentley 0:735ec5eb58d0 43 float dutyA = 1.0f; //100%
ABentley 0:735ec5eb58d0 44 float dutyB = 1.0f; //100%
ABentley 0:735ec5eb58d0 45
ABentley 0:735ec5eb58d0 46 // Distance Measurement
ABentley 0:735ec5eb58d0 47 float distanceA ;
ABentley 0:735ec5eb58d0 48 float distanceB ;
ABentley 0:735ec5eb58d0 49 float speedA ;
ABentley 0:735ec5eb58d0 50 float speedB ;
ABentley 0:735ec5eb58d0 51 float wheel_spacing = 0.128;
ABentley 0:735ec5eb58d0 52
ABentley 0:735ec5eb58d0 53 // Completed Loop
ABentley 0:735ec5eb58d0 54 int loop = 0;
ABentley 0:735ec5eb58d0 55
ABentley 0:735ec5eb58d0 56
ABentley 0:735ec5eb58d0 57 void stopmotors() // Stop Motors Spinning
ABentley 0:735ec5eb58d0 58 {
ABentley 0:735ec5eb58d0 59 PWMA.write(0.0f); //0% duty cycle
ABentley 0:735ec5eb58d0 60 PWMB.write(0.0f); //0% duty cycle
ABentley 0:735ec5eb58d0 61 }
ABentley 0:735ec5eb58d0 62
ABentley 0:735ec5eb58d0 63 void set_distanceA()
ABentley 0:735ec5eb58d0 64 {
ABentley 0:735ec5eb58d0 65 // Hall Effect Measuring Distance for Motor A
ABentley 0:735ec5eb58d0 66 distanceA += (wheelc/6);
ABentley 0:735ec5eb58d0 67 terminal.printf("A =%f\n\r",distanceA); //Print to terminal
ABentley 0:735ec5eb58d0 68 }
ABentley 0:735ec5eb58d0 69
ABentley 0:735ec5eb58d0 70 void set_distanceB()
ABentley 0:735ec5eb58d0 71 {
ABentley 0:735ec5eb58d0 72 // Hall Effect Measuring Distance for Motor B
ABentley 0:735ec5eb58d0 73 distanceB += (wheelc/6);
ABentley 0:735ec5eb58d0 74 terminal.printf("B =%f\n\r",distanceB); // Print to Terminal Distance Moved so Far
ABentley 0:735ec5eb58d0 75 }
ABentley 0:735ec5eb58d0 76
ABentley 0:735ec5eb58d0 77
ABentley 0:735ec5eb58d0 78
ABentley 0:735ec5eb58d0 79 int forward(float distance, float duty) // Move Forwards (distance,speed)
ABentley 0:735ec5eb58d0 80 {
ABentley 0:735ec5eb58d0 81 // Set Motor Direction Forward
ABentley 0:735ec5eb58d0 82 DIRA = FORWARD;
ABentley 0:735ec5eb58d0 83 DIRB = FORWARD;
ABentley 0:735ec5eb58d0 84
ABentley 0:735ec5eb58d0 85 // Set Motor Speed to Input Speed
ABentley 0:735ec5eb58d0 86 PWMA.write(duty); //Set Duty Cycle (%)
ABentley 0:735ec5eb58d0 87 PWMB.write(duty); //Set Duty Cycle (%)
ABentley 0:735ec5eb58d0 88
ABentley 0:735ec5eb58d0 89 // Reset Total Distance Travelled
ABentley 0:735ec5eb58d0 90 distanceA = 0;
ABentley 0:735ec5eb58d0 91 distanceB = 0;
ABentley 0:735ec5eb58d0 92 timer.reset();
ABentley 0:735ec5eb58d0 93 timer.start();
ABentley 0:735ec5eb58d0 94
ABentley 0:735ec5eb58d0 95 // Wait for Set Distance to be Reached
ABentley 0:735ec5eb58d0 96 while (((distanceA+distanceB)/2) < distance) {
ABentley 0:735ec5eb58d0 97 } // Using Average Distance for More Accurate Result
ABentley 0:735ec5eb58d0 98 return 1;
ABentley 0:735ec5eb58d0 99 }
ABentley 0:735ec5eb58d0 100
ABentley 0:735ec5eb58d0 101 int turn(float degrees, float duty, int direction) // (Degrees of Turn, Speed, (Anti/Clock)wise)
ABentley 0:735ec5eb58d0 102
ABentley 0:735ec5eb58d0 103 {
ABentley 0:735ec5eb58d0 104 // Calculate Turn Distance
ABentley 0:735ec5eb58d0 105 float distance=0;
ABentley 0:735ec5eb58d0 106 distance=((float)degreel*degrees);
ABentley 0:735ec5eb58d0 107 //Set Initial Motor Direction
ABentley 0:735ec5eb58d0 108 DIRA = FORWARD;
ABentley 0:735ec5eb58d0 109 DIRB = FORWARD;
ABentley 0:735ec5eb58d0 110
ABentley 0:735ec5eb58d0 111 // Test for Loop Completion to Enter Victory Spin
ABentley 0:735ec5eb58d0 112 if(direction==REVERSE) {
ABentley 0:735ec5eb58d0 113 DIRA = REVERSE;
ABentley 0:735ec5eb58d0 114 DIRB = REVERSE;
ABentley 0:735ec5eb58d0 115 }
ABentley 0:735ec5eb58d0 116
ABentley 0:735ec5eb58d0 117 // Set Motor Speed for Outside Wheel
ABentley 0:735ec5eb58d0 118 PWMA.write(duty); //Set duty cycle (%)
ABentley 0:735ec5eb58d0 119 PWMB.write(0.0f); //Set duty cycle (%)
ABentley 0:735ec5eb58d0 120
ABentley 0:735ec5eb58d0 121 // Reset Distance Travelled
ABentley 0:735ec5eb58d0 122 distanceA = 0;
ABentley 0:735ec5eb58d0 123 distanceB = 0;
ABentley 0:735ec5eb58d0 124 timer.reset();
ABentley 0:735ec5eb58d0 125 timer.start();
ABentley 0:735ec5eb58d0 126 // Wait for Turn to Complete
ABentley 0:735ec5eb58d0 127 while (distanceA < distance) {
ABentley 0:735ec5eb58d0 128 }
ABentley 0:735ec5eb58d0 129 return 1;
ABentley 0:735ec5eb58d0 130 }
ABentley 0:735ec5eb58d0 131
ABentley 0:735ec5eb58d0 132 int main()
ABentley 0:735ec5eb58d0 133 {
ABentley 0:735ec5eb58d0 134 // Configure the Terminal to High Speed
ABentley 0:735ec5eb58d0 135 terminal.baud(115200);
ABentley 0:735ec5eb58d0 136
ABentley 0:735ec5eb58d0 137 HEA1.rise(set_distanceA);
ABentley 0:735ec5eb58d0 138 HEB1.rise(set_distanceB);
ABentley 0:735ec5eb58d0 139 // Set Initial Motor Speed to Stop
ABentley 0:735ec5eb58d0 140 stopmotors();
ABentley 0:735ec5eb58d0 141 PWMA.period_ms(10);
ABentley 0:735ec5eb58d0 142 PWMB.period_ms(10);
ABentley 0:735ec5eb58d0 143 while(1) {
ABentley 0:735ec5eb58d0 144 // Wait for Switch Press
ABentley 0:735ec5eb58d0 145 while (SW1 == PRESSED) {
ABentley 0:735ec5eb58d0 146 wait(0.01);
ABentley 0:735ec5eb58d0 147 while(SW1 == RELEASED) {
ABentley 0:735ec5eb58d0 148
ABentley 0:735ec5eb58d0 149 // Navigate Course
ABentley 0:735ec5eb58d0 150 for (int i = 0; i<2; i++) {
ABentley 0:735ec5eb58d0 151 forward(12,50); // Move Forward 1.2m (extra .2 to avoid cones)
ABentley 0:735ec5eb58d0 152 turn(90,50,1); // Turn 90°
ABentley 0:735ec5eb58d0 153 forward(6,50); // Move Forward .6m (extra .1 to avoid cones)
ABentley 0:735ec5eb58d0 154 turn(90,50,1); // Turn 90°
ABentley 0:735ec5eb58d0 155 }
ABentley 0:735ec5eb58d0 156
ABentley 0:735ec5eb58d0 157 stopmotors();
ABentley 0:735ec5eb58d0 158 wait(0.5);
ABentley 0:735ec5eb58d0 159
ABentley 0:735ec5eb58d0 160 //Victory Spin
ABentley 0:735ec5eb58d0 161 turn(360,50,0); //Turn 360 degrees in Reverse
ABentley 0:735ec5eb58d0 162
ABentley 0:735ec5eb58d0 163 stopmotors();
ABentley 0:735ec5eb58d0 164 break; // End Program, Prevents Looping
ABentley 0:735ec5eb58d0 165 }
ABentley 0:735ec5eb58d0 166 }
ABentley 0:735ec5eb58d0 167 }
ABentley 0:735ec5eb58d0 168 }