Diff: motions.h

Revision:

1:f4e3365155e1

diff -r e79700919e2e -r f4e3365155e1 motions.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motions.h	Sun Oct 18 18:23:58 2015 +0000
@@ -0,0 +1,82 @@
+
+void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
+{
+    distanceA=0;
+    distanceB=0;
+    speedA=0;
+    speedB=0;
+    timer.reset();
+    timer.start();
+}
+
+void stopmotors() //stop motors dead
+{
+    PWMA.write(0.0f);       //0% duty cycle
+    PWMB.write(0.0f);       //0% duty cycle
+}
+
+void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
+{
+
+    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
+    ResetDistanceTimeSpeed();
+
+    // Set initial motor to be altered by speed code
+    PWMA.write(0.1f*speed);          //Set duty cycle (%)
+    PWMB.write(0.1*speed);          //Set duty cycle (%)
+
+
+    //wait for run to complete byy checking if distance has been covered yet
+    while (((distanceA+distanceB)/2) < distance) {
+        //maintain desired speed
+        HuntSpeeds(speed, speed);
+    }
+    return 1;
+}
+
+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 * turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
+    float C2 = (pi * 2 * (turn_radius-wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
+    //Set Initial Motor Direction
+    DIRA = FORWARD;
+    DIRB = FORWARD;
+
+    // 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
+        }
+    }
+    return 1;
+}
\ No newline at end of file