Edwin Cho / Mbed 2 deprecated FLOW_DERP

DERPS

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of STRAIGHT_DRIVE_NO_SEP by Antoine Laurens

LOCOMOTION.cpp@15:7729da55873a, 2016-03-24 (annotated)

Committer:
12104404
Date:
Thu Mar 24 07:20:51 2016 +0000
Revision:
15:7729da55873a
Parent:
6:0602a9e8118b
Child:
16:d6f15a13c3aa
how to combine x y control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 6:0602a9e8118b 1 #include "LOCOMOTION.h"
12104404 15:7729da55873a 2
12104404 15:7729da55873a 3 LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
12104404 15:7729da55873a 4 _m1f(motor1F), _m1b(motor2F), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
12104404 15:7729da55873a 5 {
12104404 15:7729da55873a 6 _m1f=0;
12104404 15:7729da55873a 7 _m1b=0;
12104404 15:7729da55873a 8 _m2f=0;
12104404 15:7729da55873a 9 _m2b=0;
12104404 15:7729da55873a 10 _m1dir=0;
12104404 15:7729da55873a 11 _m2dir=0;
12104404 15:7729da55873a 12 }
12104404 15:7729da55873a 13
12104404 15:7729da55873a 14 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
12104404 15:7729da55873a 15 {
12104404 15:7729da55873a 16 float s = 0;
12104404 15:7729da55873a 17 int diff = 0;
12104404 15:7729da55873a 18 diff = 180-wrap(target);
12104404 15:7729da55873a 19 if(abs(wrap(current+diff)-180)<=error)
12104404 15:7729da55873a 20 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 21 else
12104404 15:7729da55873a 22 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 23 if(wrap(current+diff)>180+error) {
12104404 15:7729da55873a 24
12104404 15:7729da55873a 25 } else if(wrap(current+diff)<180-error) {
12104404 15:7729da55873a 26
12104404 15:7729da55873a 27 } else {
12104404 15:7729da55873a 28 s = 0;
12104404 15:7729da55873a 29 }
12104404 15:7729da55873a 30 switch(mode) {
12104404 15:7729da55873a 31 case ANGLE_TURN:
12104404 15:7729da55873a 32
12104404 15:7729da55873a 33 break;
12104404 15:7729da55873a 34 case ANGLE_BIAS:
12104404 15:7729da55873a 35
12104404 15:7729da55873a 36 break;
12104404 15:7729da55873a 37 }
12104404 15:7729da55873a 38 }
12104404 15:7729da55873a 39
12104404 15:7729da55873a 40 int LOCOMOTION::wrap(int num)
12104404 15:7729da55873a 41 {
12104404 15:7729da55873a 42 return num%360;
12104404 15:7729da55873a 43 }
12104404 6:0602a9e8118b 44 /*
12104404 15:7729da55873a 45 void setAngle(int angle)
12104404 15:7729da55873a 46 {
12104404 15:7729da55873a 47 float s = 0;
12104404 15:7729da55873a 48 int diff = 0;
12104404 15:7729da55873a 49 diff = 180-wrap(angle);
12104404 15:7729da55873a 50 if(abs(wrap(xya.a+diff)-180)<=5)
12104404 15:7729da55873a 51 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 52 else
12104404 15:7729da55873a 53 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 54 motor1F=s;
12104404 15:7729da55873a 55 motor1B=s;
12104404 15:7729da55873a 56 motor2F=s;
12104404 15:7729da55873a 57 motor2B=s;
12104404 15:7729da55873a 58 if(wrap(xya.a+diff)>180+2) {
12104404 15:7729da55873a 59 dir1=1;
12104404 15:7729da55873a 60 dir2=0;
12104404 15:7729da55873a 61 } else if(wrap(xya.a+diff)<180-2) {
12104404 15:7729da55873a 62 dir1=0;
12104404 15:7729da55873a 63 dir2=1;
12104404 15:7729da55873a 64 } else {
12104404 15:7729da55873a 65 motor1F=0;
12104404 15:7729da55873a 66 motor1B=0;
12104404 15:7729da55873a 67 motor2F=0;
12104404 15:7729da55873a 68 motor2B=0;
12104404 15:7729da55873a 69 }*/