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@16:d6f15a13c3aa, 2016-03-27 (annotated)

Committer:
12104404
Date:
Sun Mar 27 02:39:07 2016 +0000
Revision:
16:d6f15a13c3aa
Parent:
15:7729da55873a
Child:
17:2f89826b5679
ANGLE PORTED and tested

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 16:d6f15a13c3aa 4 _m1f(motor1F), _m1b(motor1B), _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 switch(mode) {
12104404 15:7729da55873a 24 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 25 if(wrap(current+diff)>180+error) {
12104404 16:d6f15a13c3aa 26 _m1dir=1;
12104404 16:d6f15a13c3aa 27 _m2dir=0;
12104404 16:d6f15a13c3aa 28 _m1f=s;
12104404 16:d6f15a13c3aa 29 _m1b=s;
12104404 16:d6f15a13c3aa 30 _m2f=s;
12104404 16:d6f15a13c3aa 31 _m2b=s;
12104404 16:d6f15a13c3aa 32 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 33 _m1dir=0;
12104404 16:d6f15a13c3aa 34 _m2dir=1;
12104404 16:d6f15a13c3aa 35 _m1f=s;
12104404 16:d6f15a13c3aa 36 _m1b=s;
12104404 16:d6f15a13c3aa 37 _m2f=s;
12104404 16:d6f15a13c3aa 38 _m2b=s;
12104404 16:d6f15a13c3aa 39 } else {
12104404 16:d6f15a13c3aa 40 s=0;
12104404 16:d6f15a13c3aa 41 _m1f=s;
12104404 16:d6f15a13c3aa 42 _m1b=s;
12104404 16:d6f15a13c3aa 43 _m2f=s;
12104404 16:d6f15a13c3aa 44 _m2b=s;
12104404 16:d6f15a13c3aa 45 return true;
12104404 16:d6f15a13c3aa 46 }
12104404 15:7729da55873a 47 break;
12104404 15:7729da55873a 48 case ANGLE_BIAS:
12104404 15:7729da55873a 49
12104404 15:7729da55873a 50 break;
12104404 15:7729da55873a 51 }
12104404 16:d6f15a13c3aa 52 return false;
12104404 15:7729da55873a 53 }
12104404 15:7729da55873a 54
12104404 15:7729da55873a 55 int LOCOMOTION::wrap(int num)
12104404 15:7729da55873a 56 {
12104404 15:7729da55873a 57 return num%360;
12104404 15:7729da55873a 58 }
12104404 6:0602a9e8118b 59 /*
12104404 15:7729da55873a 60 void setAngle(int angle)
12104404 15:7729da55873a 61 {
12104404 15:7729da55873a 62 float s = 0;
12104404 15:7729da55873a 63 int diff = 0;
12104404 15:7729da55873a 64 diff = 180-wrap(angle);
12104404 15:7729da55873a 65 if(abs(wrap(xya.a+diff)-180)<=5)
12104404 15:7729da55873a 66 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 67 else
12104404 15:7729da55873a 68 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 69 motor1F=s;
12104404 15:7729da55873a 70 motor1B=s;
12104404 15:7729da55873a 71 motor2F=s;
12104404 15:7729da55873a 72 motor2B=s;
12104404 15:7729da55873a 73 if(wrap(xya.a+diff)>180+2) {
12104404 15:7729da55873a 74 dir1=1;
12104404 15:7729da55873a 75 dir2=0;
12104404 15:7729da55873a 76 } else if(wrap(xya.a+diff)<180-2) {
12104404 15:7729da55873a 77 dir1=0;
12104404 15:7729da55873a 78 dir2=1;
12104404 15:7729da55873a 79 } else {
12104404 15:7729da55873a 80 motor1F=0;
12104404 15:7729da55873a 81 motor1B=0;
12104404 15:7729da55873a 82 motor2F=0;
12104404 15:7729da55873a 83 motor2B=0;
12104404 15:7729da55873a 84 }*/