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@19:2dd81b864e14, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 05:48:37 2016 +0000
Revision:
19:2dd81b864e14
Parent:
18:f9012e93edb8
Child:
23:455f7da3dd7a
Cleaning up code;

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