Diff: main.cpp

Revision:

0:d1ef26d38f28

diff -r 000000000000 -r d1ef26d38f28 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 16 14:45:44 2015 +0000
@@ -0,0 +1,254 @@
+#include "mbed.h"
+
+#define wheelc 0.1759292
+#define pi 3.141592654
+#define degreel 0.021988888
+//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 = 0.100f; //100%
+float dutyB = 0.100f; //100%
+
+//distance measurement
+float distanceA ;
+float distanceB ;
+float speedA ;
+float speedB ;
+float pretimerA;
+float afttimerA;
+float pretimerB;
+float afttimerB;
+float wheel_spacing = 0.128;
+float lastinterupttimeA = 0.000f;
+float lastinterupttimeB = 0.000f;
+
+//Completed Loop
+int loop=0;
+
+int turn();
+void ResetDistanceTimeSpeed()
+{
+    distanceA=0;
+    distanceB=0;
+    speedA=0;
+    speedB=0;
+    pretimerA=0;
+    pretimerB=0;
+    afttimerA=0;
+    afttimerB=0;
+    timer.reset();
+    timer.start();
+}
+
+void stopmotors()
+{
+    PWMA.write(0.0f);       //0% duty cycle
+    PWMB.write(0.0f);       //0% duty cycle
+}
+
+int forward(float distance, float speed)
+{
+    //add forward to input with switch for scalability
+    // Set motor direction forward
+    DIRA = FORWARD;
+    DIRB = FORWARD;
+
+    //reset distance
+    ResetDistanceTimeSpeed();
+
+    // Set motor speed to input speed
+    PWMA.write(0.1f);          //Set duty cycle (%)
+    PWMB.write(0.1f);          //Set duty cycle (%)
+
+
+    //wait for run to complete
+    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);
+        }
+        wait(0.05);
+    }
+    return 1;
+}
+
+int turn(float degrees, float speed, 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;
+
+// Set Motor Speed for Outside Wheel
+    PWMA.write(0.0f);          //Set duty cycle (%)
+    PWMB.write(0.0f);          //Set duty cycle (%)
+
+    // Test for Loop Completion to Enter Victory Spin
+    if(direction==REVERSE) {
+        DIRA = REVERSE;
+        DIRB = FORWARD;
+        PWMB.write(0.0f);          //Set duty cycle (%)
+    }
+
+
+
+    // Reset Distance Travelled
+    ResetDistanceTimeSpeed();
+    // Wait for Turn to Complete
+    while (distanceA < distance) {
+        if (speedA<speed) {
+
+            dutyA += (float)0.0051;
+            PWMA.write(dutyA);
+        }
+        if( speedA>speed) {
+            dutyA -= (float)0.0051;
+            PWMA.write(dutyA);
+        }
+
+        if(direction==REVERSE) {
+            if (speedB<(speed/2)) {
+
+                dutyB += (float)0.0051;
+                PWMB.write(dutyB);
+            }
+            if( speedB>(speed/2)) {
+                dutyB -= (float)0.0051;
+                PWMB.write(dutyB);
+            }
+        }
+        wait(0.1);
+    }
+    return 1;
+}
+
+
+
+void set_distanceA()
+{
+    float time = 0;
+    float interupttime = timer.read_us();
+
+
+    if(interupttime-lastinterupttimeA > 5) {
+
+        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 = 0.166667f/(time);              //distance/time = average speed for last 6th rotation
+        pretimerA = afttimerA;                  //update pretimer for next calculation of time
+        terminal.printf("speedA %f : time %f\n\r", speedA, time);
+    }
+    lastinterupttimeA = interupttime;
+}
+
+void set_distanceB()
+{
+    float time = 0;
+    float interupttime = timer.read_us();
+
+    if(interupttime-lastinterupttimeB > 5) {
+        afttimerB = timer.read();
+        distanceB += (wheelc/6);
+        time = afttimerB - pretimerB;
+        speedB = 0.166667f/time;
+        pretimerB = afttimerB;
+        terminal.printf("speedB %f : time %f\n\r", speedB, time);
+    }
+    lastinterupttimeB = interupttime;
+}
+
+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,10);
+                    //terminal.printf("turn\n\r");
+                    turn(90,10,0);
+                    //terminal.printf("forward\n\r");
+                    forward(7,10);
+                    //terminal.printf("turn\n\r");
+                    turn(90,10,0);
+                }
+
+                stopmotors();
+
+                wait(0.5);
+
+                //victory spin
+                turn(180,40,1);
+
+                stopmotors();
+                break;
+            }
+        }
+    }
+
+}
+