Localization
Dependencies: BNO055_fusion mbed
LOCOMOTION.cpp
00001 #include "LOCOMOTION.h" 00002 00003 LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): 00004 _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) 00005 { 00006 _m1f=0; 00007 _m1b=0; 00008 _m2f=0; 00009 _m2b=0; 00010 _m1dir=0; 00011 _m2dir=0; 00012 } 00013 00014 bool LOCOMOTION::setXPos(int target, int current, int error, int angle) 00015 { 00016 //s = 0; 00017 if(abs(current-target)<=error) 00018 s=0.07; 00019 else 00020 s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; 00021 if(current>target+error) { 00022 if(angle==0) { 00023 _m1dir=1; 00024 _m2dir=1; 00025 } else { 00026 _m1dir=0; 00027 _m2dir=0; 00028 } 00029 _m1f=s; 00030 _m1b=s; 00031 _m2f=s; 00032 _m2b=s; 00033 } else if(current<target-error) { 00034 if(angle==0) { 00035 _m1dir=0; 00036 _m2dir=0; 00037 } else { 00038 _m1dir=1; 00039 _m2dir=1; 00040 } 00041 _m1f=s; 00042 _m1b=s; 00043 _m2f=s; 00044 _m2b=s; 00045 } else { 00046 s=0; 00047 _m1f=s; 00048 _m1b=s; 00049 _m2f=s; 00050 _m2b=s; 00051 return true; 00052 } 00053 return false; 00054 } 00055 00056 bool LOCOMOTION::setYPos(int target, int current, int error, int angle) 00057 { 00058 //float s = 0; 00059 if(abs(current-target)<=error) 00060 s=0.50; 00061 else 00062 s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; 00063 if(current>target+error) { 00064 //_m1dir=1; 00065 //_m2dir=1; 00066 if(angle==0) { 00067 _m1f=_m1f*(1+s); 00068 _m1b=_m1b*(1+s); 00069 } else { 00070 _m2f=_m2f*(1+s); 00071 _m2b=_m2b*(1+s); 00072 } 00073 } else if(current<target-error) { 00074 //_m1dir=0; 00075 //_m2dir=0; 00076 if(angle==0) { 00077 _m2f=_m2f*(1+s); 00078 _m2b=_m2b*(1+s); 00079 } else { 00080 _m1f=_m1f*(1+s); 00081 _m1b=_m1b*(1+s); 00082 } 00083 } else { 00084 s=0; 00085 00086 return true; 00087 } 00088 return false; 00089 } 00090 00091 bool LOCOMOTION::setAngle(int target, int current, int error, int mode) 00092 { 00093 s = 0; 00094 int diff = 0; 00095 diff = 180-wrap(target); 00096 if(abs(wrap(current+diff)-180)<=error) 00097 s=SPEED_TURN_MIN; 00098 else 00099 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; 00100 switch(mode) { 00101 case ANGLE_TURN: 00102 if(wrap(current+diff)>180+error) { 00103 _m1dir=1; 00104 _m2dir=0; 00105 _m1f=s; 00106 _m1b=s; 00107 _m2f=s; 00108 _m2b=s; 00109 } else if(wrap(current+diff)<180-error) { 00110 _m1dir=0; 00111 _m2dir=1; 00112 _m1f=s; 00113 _m1b=s; 00114 _m2f=s; 00115 _m2b=s; 00116 } else { 00117 s=0; 00118 _m1f=s; 00119 _m1b=s; 00120 _m2f=s; 00121 _m2b=s; 00122 return true; 00123 } 00124 break; 00125 case ANGLE_BIAS: 00126 00127 break; 00128 } 00129 return false; 00130 } 00131 00132 int LOCOMOTION::wrap(int num) 00133 { 00134 return num%360; 00135 }
Generated on Fri Jul 15 2022 21:18:53 by 1.7.2