Revision 0:7118ab174b9c, committed 2015-10-15

Comitter:

benparkes

Date:

Thu Oct 15 12:13:00 2015 +0000

Commit message:

speeds 15/10/15

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 15 12:13:00 2015 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+
+#define wheelc 0.1759292
+#define pi 3.141592654
+#define arcl 1.979203372
+//Status LED
+DigitalOut led(LED1);
+
+//Motor PWM (speed)
+PwmOut PWMA(PA_8);
+PwmOut PWMB(PB_4);
+
+//Motor Direction
+DigitalOut DIRA(PA_9);
+DigitalOut DIRB(PB_10);
+
+//Hall-Effect Sensor Inputs
+InterruptIn HEA1(PB_2);
+DigitalIn HEA2(PB_1);
+InterruptIn HEB1(PB_15);
+DigitalIn HEB2(PB_14);
+
+//On board switch
+DigitalIn SW1(USER_BUTTON);
+
+//Use the serial object so we can use higher speeds
+Serial terminal(USBTX, USBRX);
+
+//Timer used for measuring speeds
+Timer timer;
+
+//Enumerated types
+enum DIRECTION   {FORWARD=0, REVERSE};
+enum PULSE           {NOPULSE=0, PULSE};
+enum SWITCHSTATE {PRESSED=0, RELEASED};
+
+//Debug GPIO
+DigitalOut probe(D10);
+
+//Duty cycles
+float dutyA = 1.0f; //100%
+float dutyB = 1.0f; //100%
+
+//distance measurement
+float distanceA ;
+float distanceB ;
+float speedA ;
+float speedB ;
+float pretimerA;
+float afttimerA;
+float pretimerB;
+float afttimerB;
+float wheel_spacing = 0.128;
+
+//Completed Loop
+int loop=0;
+
+
+void ResetDistanceTimeSpeed()
+{
+    distanceA=0;
+    distanceB=0;
+    speedA=0;
+    speedB=0;
+    pretimerA=0;
+    pretimerB=0;
+    afttimerA=0;
+    afttimerB=0;
+}
+
+void stopmotors()
+{
+    PWMA.write(0.0f);       //0% duty cycle
+    PWMB.write(0.0f);       //0% duty cycle
+}
+
+int forward(float distance, float speed, float duty)
+{
+    //add forward to input with switch for scalability
+    int i;
+    // Set motor direction forward
+    DIRA = FORWARD;
+    DIRB = FORWARD;
+
+    // Set motor speed to input speed
+    PWMA.write(duty);          //Set duty cycle (%)
+    PWMB.write(duty);          //Set duty cycle (%)
+
+    //reset distance
+    ResetDistanceTimeSpeed();
+
+    //wait for run to complete
+    while (((distanceA+distanceB)/2) < distance) {
+        switch {
+        case(speedA<speed) {
+                    duty += (0.1*duty);
+                    PWMA.write(duty);
+                }
+
+            case(speedA>speed) {
+                    duty+=(0.1*duty);
+                    PWMA.write(duty);
+                }
+                return 1;
+        }
+
+        int turn(float distance, float duty, int loop) {
+            //Set initial motor direction
+            DIRA = FORWARD;
+            DIRB = FORWARD;
+
+            //test for loop completion
+            if(loop==1) {
+                DIRA = REVERSE;
+                DIRB = REVERSE;
+            }
+            /*
+            float C1 = (pi * 2 * distance_outside_wheel_turn_point)*(degrees/360);
+            float C2 = (pi * 2 * (distance_outside_wheel_turn_point-wheel_spacing))*(degrees/360);
+            */
+            //Set motor speed
+            PWMA.write(duty);          //Set duty cycle (%)
+            PWMB.write(0.0f);          //Set duty cycle (%)
+            //reset distance time and speed
+            resetDistanceTimeSpeed();
+            //wait for turn to complete
+            while (distanceA < distance) {
+            }
+            return 1;
+        }
+
+        void set_distanceA() {
+            float time = 0;
+            
+            afttimerA = timer;                      //set latest timer to equal timer
+            distanceA += (wheelc/6);                  //set distance travelled for this instruction i.e forward/turn etc
+            terminal.printf("A =%f\n\r",distanceA); // print for fault finding (delete once working)
+            time = afttimer - pretimer;             //
+            speedA = (wheelc/6)/time);
+            pretimerA = afttimerA;
+        }
+
+        void set_distanceB() {
+            float time = 0;
+            
+            afttimerB = timer;
+            distanceB += (wheelc/6);
+            terminal.printf("A =%f\n\r",distanceB);
+            time = afttimer - pretimer;
+            speedB = (wheelc/6)/time);
+            pretimerB = afttimerB;
+        }
+
+
+        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,50);
+                            //terminal.printf("turn\n\r");
+                            turn(arcl,50,0);
+                            //terminal.printf("forward\n\r");
+                            forward(6,50);
+                            //terminal.printf("turn\n\r");
+                            turn(arcl,50,0);
+                        }
+
+                        stopmotors();
+
+                        wait(0.5);
+
+                        //victory spin
+                        turn(arcl*4.5,50,1);
+
+                        stopmotors();
+                        break;
+                    }
+                }
+            }
+        }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Oct 15 12:13:00 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68
\ No newline at end of file