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@17:2f89826b5679, 2016-03-29 (annotated)

Committer:
12104404
Date:
Tue Mar 29 01:11:09 2016 +0000
Revision:
17:2f89826b5679
Parent:
16:d6f15a13c3aa
Child:
18:f9012e93edb8
working

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 17:2f89826b5679 14 bool LOCOMOTION::setXPos(int target, int current, int error)
12104404 17:2f89826b5679 15 {
12104404 17:2f89826b5679 16 //s = 0;
12104404 17:2f89826b5679 17 if(abs(current-target)<=error)
12104404 17:2f89826b5679 18 s=0.07;
12104404 17:2f89826b5679 19 else
12104404 17:2f89826b5679 20 s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
12104404 17:2f89826b5679 21 if(current>target+error) {
12104404 17:2f89826b5679 22 _m1dir=1;
12104404 17:2f89826b5679 23 _m2dir=1;
12104404 17:2f89826b5679 24 _m1f=s;
12104404 17:2f89826b5679 25 _m1b=s;
12104404 17:2f89826b5679 26 _m2f=s;
12104404 17:2f89826b5679 27 _m2b=s;
12104404 17:2f89826b5679 28 } else if(current<target-error) {
12104404 17:2f89826b5679 29 _m1dir=0;
12104404 17:2f89826b5679 30 _m2dir=0;
12104404 17:2f89826b5679 31 _m1f=s;
12104404 17:2f89826b5679 32 _m1b=s;
12104404 17:2f89826b5679 33 _m2f=s;
12104404 17:2f89826b5679 34 _m2b=s;
12104404 17:2f89826b5679 35 } else {
12104404 17:2f89826b5679 36 s=0;
12104404 17:2f89826b5679 37 _m1f=s;
12104404 17:2f89826b5679 38 _m1b=s;
12104404 17:2f89826b5679 39 _m2f=s;
12104404 17:2f89826b5679 40 _m2b=s;
12104404 17:2f89826b5679 41 return true;
12104404 17:2f89826b5679 42 }
12104404 17:2f89826b5679 43 return false;
12104404 17:2f89826b5679 44 }
12104404 17:2f89826b5679 45
12104404 17:2f89826b5679 46 bool LOCOMOTION::setYPos(int target, int current, int error)
12104404 17:2f89826b5679 47 {
12104404 17:2f89826b5679 48 //float s = 0;
12104404 17:2f89826b5679 49 if(abs(current-target)<=error)
12104404 17:2f89826b5679 50 s=0.50;
12104404 17:2f89826b5679 51 else
12104404 17:2f89826b5679 52 s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
12104404 17:2f89826b5679 53 if(current>target+error) {
12104404 17:2f89826b5679 54 //_m1dir=1;
12104404 17:2f89826b5679 55 //_m2dir=1;
12104404 17:2f89826b5679 56 _m1f=_m1f*(1+s);
12104404 17:2f89826b5679 57 _m1b=_m1b*(1+s);
12104404 17:2f89826b5679 58 } else if(current<target-error) {
12104404 17:2f89826b5679 59 //_m1dir=0;
12104404 17:2f89826b5679 60 //_m2dir=0;
12104404 17:2f89826b5679 61 _m2f=_m2f*(1+s);
12104404 17:2f89826b5679 62 _m2b=_m2b*(1+s);
12104404 17:2f89826b5679 63 } else {
12104404 17:2f89826b5679 64 s=0;
12104404 17:2f89826b5679 65
12104404 17:2f89826b5679 66 return true;
12104404 17:2f89826b5679 67 }
12104404 17:2f89826b5679 68 return false;
12104404 17:2f89826b5679 69 }
12104404 17:2f89826b5679 70
12104404 15:7729da55873a 71 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
12104404 15:7729da55873a 72 {
12104404 17:2f89826b5679 73 s = 0;
12104404 15:7729da55873a 74 int diff = 0;
12104404 15:7729da55873a 75 diff = 180-wrap(target);
12104404 15:7729da55873a 76 if(abs(wrap(current+diff)-180)<=error)
12104404 15:7729da55873a 77 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 78 else
12104404 15:7729da55873a 79 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 80 switch(mode) {
12104404 15:7729da55873a 81 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 82 if(wrap(current+diff)>180+error) {
12104404 16:d6f15a13c3aa 83 _m1dir=1;
12104404 16:d6f15a13c3aa 84 _m2dir=0;
12104404 16:d6f15a13c3aa 85 _m1f=s;
12104404 16:d6f15a13c3aa 86 _m1b=s;
12104404 16:d6f15a13c3aa 87 _m2f=s;
12104404 16:d6f15a13c3aa 88 _m2b=s;
12104404 16:d6f15a13c3aa 89 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 90 _m1dir=0;
12104404 16:d6f15a13c3aa 91 _m2dir=1;
12104404 16:d6f15a13c3aa 92 _m1f=s;
12104404 16:d6f15a13c3aa 93 _m1b=s;
12104404 16:d6f15a13c3aa 94 _m2f=s;
12104404 16:d6f15a13c3aa 95 _m2b=s;
12104404 16:d6f15a13c3aa 96 } else {
12104404 16:d6f15a13c3aa 97 s=0;
12104404 16:d6f15a13c3aa 98 _m1f=s;
12104404 16:d6f15a13c3aa 99 _m1b=s;
12104404 16:d6f15a13c3aa 100 _m2f=s;
12104404 16:d6f15a13c3aa 101 _m2b=s;
12104404 16:d6f15a13c3aa 102 return true;
12104404 16:d6f15a13c3aa 103 }
12104404 15:7729da55873a 104 break;
12104404 15:7729da55873a 105 case ANGLE_BIAS:
12104404 15:7729da55873a 106
12104404 15:7729da55873a 107 break;
12104404 15:7729da55873a 108 }
12104404 16:d6f15a13c3aa 109 return false;
12104404 15:7729da55873a 110 }
12104404 15:7729da55873a 111
12104404 15:7729da55873a 112 int LOCOMOTION::wrap(int num)
12104404 15:7729da55873a 113 {
12104404 15:7729da55873a 114 return num%360;
12104404 17:2f89826b5679 115 }