Diff: main.cpp

Revision:

0:735ec5eb58d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 15 13:43:39 2015 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+
+#define wheelc 0.1759292 // Circumference of Wheel
+#define pi 3.141592654
+#define degreel 0.021988888 // Distance Travelled in 1 Degree
+
+
+// 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 wheel_spacing = 0.128;
+
+// Completed Loop
+int loop = 0;
+
+
+void stopmotors() // Stop Motors Spinning
+{
+    PWMA.write(0.0f);           //0% duty cycle
+    PWMB.write(0.0f);           //0% duty cycle
+}
+
+void set_distanceA()
+{
+    // Hall Effect Measuring Distance for Motor A
+    distanceA += (wheelc/6);
+    terminal.printf("A =%f\n\r",distanceA); //Print to terminal
+}
+
+void set_distanceB()
+{
+    // Hall Effect Measuring Distance for Motor B
+    distanceB += (wheelc/6);
+    terminal.printf("B =%f\n\r",distanceB); // Print to Terminal Distance Moved so Far
+}
+
+
+
+int forward(float distance, float duty) // Move Forwards (distance,speed)
+{
+    // 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 Total Distance Travelled
+    distanceA = 0;
+    distanceB = 0;
+    timer.reset();
+    timer.start();
+
+    // Wait for Set Distance to be Reached
+    while (((distanceA+distanceB)/2) < distance) {
+    } // Using Average Distance for More Accurate Result
+    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
+    distanceA = 0;
+    distanceB = 0;
+    timer.reset();
+    timer.start();
+    // Wait for Turn to Complete
+    while (distanceA < distance) {
+    }
+    return 1;
+}
+
+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); // Move Forward 1.2m (extra .2 to avoid cones)
+                    turn(90,50,1); // Turn 90°
+                    forward(6,50); // Move Forward .6m (extra .1 to avoid cones)
+                    turn(90,50,1); // Turn 90°
+                }
+
+                stopmotors();
+                wait(0.5);
+
+                //Victory Spin
+                turn(360,50,0); //Turn 360 degrees in Reverse
+
+                stopmotors();
+                break; // End Program, Prevents Looping
+            }
+        }
+    }
+}
\ No newline at end of file