Diff: LOCOMOTION.cpp

Revision:

15:7729da55873a

Parent:

6:0602a9e8118b

Child:

16:d6f15a13c3aa

--- a/LOCOMOTION.cpp	Thu Mar 24 03:54:29 2016 +0000
+++ b/LOCOMOTION.cpp	Thu Mar 24 07:20:51 2016 +0000
@@ -1,5 +1,69 @@
 #include "LOCOMOTION.h"
+
+LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
+    _m1f(motor1F), _m1b(motor2F), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
+{
+    _m1f=0;
+    _m1b=0;
+    _m2f=0;
+    _m2b=0;
+    _m1dir=0;
+    _m2dir=0;
+}
+
+bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
+{
+    float s = 0;
+    int diff = 0;
+    diff = 180-wrap(target);
+    if(abs(wrap(current+diff)-180)<=error)
+        s=SPEED_TURN_MIN;
+    else
+        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
+    if(wrap(current+diff)>180+error) {
+        
+    } else if(wrap(current+diff)<180-error) {
+        
+    } else {
+        s = 0;
+    }
+    switch(mode) {
+        case ANGLE_TURN:
+
+            break;
+        case ANGLE_BIAS:
+
+            break;
+    }
+}
+
+int LOCOMOTION::wrap(int num)
+{
+    return num%360;
+}
 /*
-LOCOMOTION::LOCOMOTION (PwmOut& motor1F, PwmOut& motor1B,PwmOut& motor2F, PwmOut& motor2B, DigitalOut& forward1, DigitalOut& forward2):
-    _speed.m1f(motor1F), _speed.m1b(motor1B), _speed.m2f(motor2F), _speed.m2b(motor2B){}
-    */
\ No newline at end of file
+void setAngle(int angle)
+{
+    float s = 0;
+    int diff = 0;
+    diff = 180-wrap(angle);
+    if(abs(wrap(xya.a+diff)-180)<=5)
+        s=SPEED_TURN_MIN;
+    else
+        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
+    motor1F=s;
+    motor1B=s;
+    motor2F=s;
+    motor2B=s;
+    if(wrap(xya.a+diff)>180+2) {
+        dir1=1;
+        dir2=0;
+    } else if(wrap(xya.a+diff)<180-2) {
+        dir1=0;
+        dir2=1;
+    } else {
+        motor1F=0;
+        motor1B=0;
+        motor2F=0;
+        motor2B=0;
+    }*/