Coursework Buggy
nicer code
Dependencies: mbed
Fork of
161015
by 
Coursework Buggy
Diff: main.cpp
- Revision:
- 1:f4e3365155e1
- Parent:
- 0:e79700919e2e
--- a/main.cpp Fri Oct 16 12:39:12 2015 +0000
+++ b/main.cpp Sun Oct 18 18:23:58 2015 +0000
@@ -1,6 +1,8 @@
#include "mbed.h"
+
#define wheelc 0.1759292
+#define wheel_spacing 0.128 //add this in
#define pi 3.141592654
#define degreel 0.021988888
//Status LED
@@ -16,9 +18,9 @@
//Hall-Effect Sensor Inputs
InterruptIn HEA1(PB_2);
-DigitalIn HEA2(PB_1);
+InterruptIn HEA2(PB_1);
InterruptIn HEB1(PB_15);
-DigitalIn HEB2(PB_14);
+InterruptIn HEB2(PB_14);
//On board switch
DigitalIn SW1(USER_BUTTON);
@@ -31,183 +33,271 @@
//Enumerated types
enum DIRECTION {FORWARD=0, REVERSE};
-enum PULSE {NOPULSE=0, PULSE};
+enum PULSE {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};
//Debug GPIO
DigitalOut probe(D10);
//Duty cycles
-float dutyA = 1.000f; //100%
-float dutyB = 1.000f; //100%
+float dutyA = 0.000f; //100%
+float dutyB = 0.000f; //100%
//distance measurement
float distanceA ;
float distanceB ;
float speedA ;
float speedB ;
-float pretimerA;
-float afttimerA;
-float pretimerB;
-float afttimerB;
-float wheel_spacing = 0.128;
+
+//for interrupt speed and distance calcs
+int tA1[2];
+int tA2[2];
+int tB1[2];
+int tB2[2];
+int pulse_orderA = 1;
+int pulse_orderB = 1;
+
+//interupt handler
+int HEPulse(int let,int num, int order) //tell us which hall effect HEA1= hall effect, (let, A=1, B=2), (num, 1/2), order (index possition to be filled alternates)
+{
+ float fA1;
+ float fA2;
+ float fB1;
+ float fB2;
+ int time;
+ time = timer.read_us(); //use to check for interrupt debounce
+
+ if((tA1[0]-time<5)||(tA2[0]-time<5)||(tA1[1]-time<5)||(tA2[1]-time<5)) {
+ return 1;
+ } // if less than 5us from last interupt assume debouce and ignore
+ //find right thing to do for current hall effect
+ switch(order) { //check whether to fill index possition 0/1
+ case(1) :
+ switch(let) { //check hall effect A/B
+ case(1):
+
+ switch(num) { //check hall effect 1/2
+ case (1):
+ tA1[0] = timer.read_us();
+ distanceA += (wheelc/6); //increment distance
+ pulse_orderA = 2; //toggle index to be filled
+ break;
+
+ case(2) :
+ tA2[0] = timer.read_us();
+ pulse_orderA = 2;
+ break;
+ }
+ break;
+
+
+ case(2): //check hall effect A/B
+
+ switch(num) {
+ case (1):
+ tB1[0] = timer.read_us();
+ distanceB += (wheelc/6);
+ pulse_orderB = 2;
+ break;
-//Completed Loop
-int loop=0;
+ case(2) :
+ tB2[0] = timer.read_us();
+ pulse_orderB = 2;
+ break;
+ }
+ break;
+
+ }
+ case(2) :
+ switch(let) {
+ case(1):
+
+ switch(num) {
+ case (1):
+ tA1[1] = timer.read_us();
+ distanceA += (wheelc/6);
+ fA1 = 1.0f/(( fabsf(tA1[1]-tA1[0]) ) * (float)3.0E-6);
+ pulse_orderA = 1;
+ break;
+
+ case(2) :
+ tA2[1] = timer.read_us();
+ fA2 = 1.0f/(( fabsf(tA2[1]-tA2[0]) ) * (float)3.0E-6);
+ pulse_orderA = 1;
+ break;
+ }
+ break;
+
-int turn();
-void ResetDistanceTimeSpeed()
+ case(2):
+
+ switch(num) {
+ case (1):
+ tB1[1] = timer.read_us();
+ distanceB += (wheelc/6);
+ fB1 = 1.0f/(( fabsf(tB1[1]-tB1[0]) ) * (float)3.0E-6);
+ pulse_orderB = 1;
+ break;
+
+ case(2) :
+ tB2[1] = timer.read_us();
+ fB2 = 1.0f/((fabsf( tB2[1]-tB2[0]) ) * (float)3.0E-6);
+ pulse_orderB = 1;
+ break;
+ }
+ break;
+
+ }
+
+ float fA = (fA1 + fA2)*0.5f; //calculate average frequency of hall effect A
+ float fB = (fB1 + fB2)*0.5f; //calculate average frequency of hall effect B
+ speedA = fA/20.8f; //output speeds of motors by frequency/gear box ratio
+ speedB = fB/20.8f;
+ }
+ return 1;
+}
+
+void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
{
distanceA=0;
distanceB=0;
speedA=0;
speedB=0;
- pretimerA=0;
- pretimerB=0;
- afttimerA=0;
- afttimerB=0;
timer.reset();
timer.start();
}
-void stopmotors()
+void stopmotors() //stop motors dead
{
PWMA.write(0.0f); //0% duty cycle
PWMB.write(0.0f); //0% duty cycle
}
-int forward(float distance, float speed)
+void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
{
- //add forward to input with switch for scalability
+
+ float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed
+ dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error
+ PWMA.write(dutyA); //write new duty to motor
+
+ float deltaB = target_speedB -speedB; //repeat for B motor
+ dutyB = dutyB + deltaB*0.1f;
+ PWMB.write(dutyB);
+}
+
+
+int forward(float distance, float speed) //forward motion, input distance and speed (rev/s)
+{
// Set motor direction forward
DIRA = FORWARD;
DIRB = FORWARD;
- //reset distance
+ //reset distance
ResetDistanceTimeSpeed();
-
- // Set motor speed to input speed
- PWMA.write(0.1); //Set duty cycle (%)
- PWMB.write(0.1); //Set duty cycle (%)
+
+ // Set initial motor to be altered by speed code
+ PWMA.write(0.1f*speed); //Set duty cycle (%)
+ PWMB.write(0.1f*speed); //Set duty cycle (%)
- //wait for run to complete
+ //wait for run to complete byy checking if distance has been covered yet
while (((distanceA+distanceB)/2) < distance) {
-
- if (speedA<speed){
-
- dutyA += (float)0.0051;
- PWMA.write(dutyA);
- }
- if( speedA>speed){
- dutyA -= (float)0.0051;
- PWMA.write(dutyA);
- }
-
- if (speedB<speed){
-
- dutyB += (float)0.0051;
- PWMB.write(dutyB);
- }
- if( speedB>speed){
- dutyB -= (float)0.0051;
- PWMB.write(dutyB);
- }
-
- }
- return 1;
- }
-
-int turn(float degrees, float duty, int direction) // (Degrees of Turn, Speed, (Anti/Clock)wise)
-
-{
- // Calculate Turn Distance
- float distance=0;
- distance=((float)degreel*degrees);
- //Set Initial Motor Direction
- DIRA = FORWARD;
- DIRB = FORWARD;
-
- // Test for Loop Completion to Enter Victory Spin
- if(direction==REVERSE) {
- DIRA = REVERSE;
- DIRB = REVERSE;
- }
-
- // Set Motor Speed for Outside Wheel
- PWMA.write(duty); //Set duty cycle (%)
- PWMB.write(0.0f); //Set duty cycle (%)
-
- // Reset Distance Travelled
- ResetDistanceTimeSpeed();
- // Wait for Turn to Complete
- while (distanceA < distance) {
+ //maintain desired speed
+ HuntSpeeds(speed, speed);
}
return 1;
}
- void set_distanceA() {
- float time = 0;
+int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m))
+
+{
+ // Calculate Turn Distance
+ float C1 = (pi * 2 * (float)turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
+ float C2 = (pi * 2 * ((float)turn_radius-(float)wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
+ //Set Initial Motor Direction
+ DIRA = FORWARD;
+ DIRB = FORWARD;
- afttimerA = timer.read(); //set latest timer to equal timer
- distanceA += (wheelc/6); //set distance travelled for this instruction i.e forward/turn etc
- time = afttimerA - pretimerA; //work out time taken for last 6th of a rotation
- speedA = (time)/6; //distance/time = average speed for last 6th rotation
- pretimerA = afttimerA; //update pretimer for next calculation of time
- terminal.printf("speedA %f\n\r", speedA);
+ // Test for direction to Enter Victory Spin
+ if(direction==REVERSE) {
+ DIRA = REVERSE;
+ DIRB = FORWARD;//victory spin opposite wheels
+ }
+ // Reset Distance Travelled
+ ResetDistanceTimeSpeed();
+
+ // Wait for Turn to Complete by checking whether outer wheel completed spin yet
+ while (distanceA < C1) {
+ if(direction==FORWARD) {
+ HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel
+ }
+
+ if(direction==REVERSE) {
+ HuntSpeeds(speed,speed); //set speeds same for reverse spin
+ }
}
-
- void set_distanceB() {
- float time = 0;
+ return 1;
+}
+//interupt handlers
+void HEA1I(void)
+{
+ HEPulse(1,1,pulse_orderA);
+}
+void HEA2I(void)
+{
+ HEPulse(1,2,pulse_orderA);
+}
+void HEB1I(void)
+{
+ HEPulse(2,1,pulse_orderB);
+}
+ void HEB2I(void)
+{
+ HEPulse(2,2,pulse_orderB);
+}
- afttimerB = timer.read();
- distanceB += (wheelc/6);
- time = afttimerB - pretimerB;
- speedB = time/6;
- pretimerB = afttimerB;
- terminal.printf("speedB %f\n\r", speedB);
+int main()
+{
+ //Configure the terminal to high speed
+ terminal.baud(115200);
+ // initiate interupts to call interupt code
+ HEA1.rise(&HEA1I);
+ HEA2.rise(&HEA2I);
+ HEB1.rise(&HEB1I);
+ HEB2.rise(&HEB2I);
+ //Set initial motor speed to stop
+ stopmotors();
+ //set motor frequency
+ PWMA.period_ms(10);
+ PWMB.period_ms(10);
+ ResetDistanceTimeSpeed();
+ while(1) {
+ //wait for switch press
+ while (SW1 == PRESSED) {
+ wait(0.01);
+ while(SW1 == RELEASED) {
+
+ //navigate course
+ for (int i = 0; i<2; i++) {
+ //forward 1m
+ forward(12,1); //distance, speed in rotations/s
+ //turn 90
+ turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+ //forward 1/2m
+ forward(7,1);//distance, speed in rotations/s
+ //turn 90
+ turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+ }
+ //repeat twice to end up back in starting possition
+
+ //victory spin
+ turn(365,0.5,1,0.001);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+
+ stopmotors();
+ break;
+ }
+ }
}
-
- int main() {
- //Configure the terminal to high speed
- terminal.baud(115200);
-
- HEA1.rise(set_distanceA);
- HEB1.rise(set_distanceB);
- //Set initial motor speed to stop
- stopmotors();
- PWMA.period_ms(10);
- PWMB.period_ms(10);
- while(1) {
- //wait for switch press
- while (SW1 == PRESSED) {
- wait(0.01);
- while(SW1 == RELEASED) {
+}
- //navigate course
- for (int i = 0; i<2; i++) {
- forward(12,0.01);
- //terminal.printf("turn\n\r");
- turn(100,1,0);
- //terminal.printf("forward\n\r");
- forward(7,0.01);
- //terminal.printf("turn\n\r");
- turn(100,1,0);
- }
-
- stopmotors();
-
- wait(0.5);
-
- //victory spin
- turn(365,1,1);
-
- stopmotors();
- break;
- }
- }
- }
-
- }
-