WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
main.cpp@1:f102831401a8, 2019-12-17 (annotated)
- Committer:
- sgrsn
- Date:
- Tue Dec 17 04:41:19 2019 +0000
- Revision:
- 1:f102831401a8
- Parent:
- 0:f1459eec7228
- Child:
- 2:83fa142c5bcc
Fix for Self-localization Estimate
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:f1459eec7228 | 1 | #include "mbed.h" |
sgrsn | 0:f1459eec7228 | 2 | #include <string> |
sgrsn | 0:f1459eec7228 | 3 | #include "i2cmaster.h" |
sgrsn | 0:f1459eec7228 | 4 | #include "JY901.h" |
sgrsn | 0:f1459eec7228 | 5 | #include "PID.h" |
sgrsn | 0:f1459eec7228 | 6 | #include "MakeSequencer.h" |
sgrsn | 0:f1459eec7228 | 7 | #include "define.h" |
sgrsn | 0:f1459eec7228 | 8 | |
sgrsn | 0:f1459eec7228 | 9 | #include "TextLCD.h" |
sgrsn | 0:f1459eec7228 | 10 | |
sgrsn | 0:f1459eec7228 | 11 | |
sgrsn | 0:f1459eec7228 | 12 | // MakeSequencer |
sgrsn | 0:f1459eec7228 | 13 | #define SIZE 6 |
sgrsn | 0:f1459eec7228 | 14 | #define ArraySize(array) (sizeof(array) / sizeof(array[0])) |
sgrsn | 0:f1459eec7228 | 15 | |
sgrsn | 1:f102831401a8 | 16 | float wheel_d = 127; // メカナム直径[mm] |
sgrsn | 1:f102831401a8 | 17 | float wheel_r = 63.5; |
sgrsn | 1:f102831401a8 | 18 | float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) |
sgrsn | 0:f1459eec7228 | 19 | |
sgrsn | 1:f102831401a8 | 20 | float DEG_TO_RAD = PI/180.0; |
sgrsn | 1:f102831401a8 | 21 | float RAD_TO_DEG = 180.0/PI; |
sgrsn | 1:f102831401a8 | 22 | |
sgrsn | 1:f102831401a8 | 23 | int controlMotor(int ch, int frequency); |
sgrsn | 0:f1459eec7228 | 24 | void coastAllMotor(); |
sgrsn | 0:f1459eec7228 | 25 | void controlFrequencyFromTerminal(); |
sgrsn | 0:f1459eec7228 | 26 | void serialRead(); |
sgrsn | 1:f102831401a8 | 27 | void controlFromWASD(); |
sgrsn | 1:f102831401a8 | 28 | |
sgrsn | 1:f102831401a8 | 29 | class CountWheel |
sgrsn | 1:f102831401a8 | 30 | { |
sgrsn | 1:f102831401a8 | 31 | public: |
sgrsn | 1:f102831401a8 | 32 | CountWheel(Timer *t) |
sgrsn | 1:f102831401a8 | 33 | { |
sgrsn | 1:f102831401a8 | 34 | _t = t; |
sgrsn | 1:f102831401a8 | 35 | _t -> start(); |
sgrsn | 1:f102831401a8 | 36 | } |
sgrsn | 1:f102831401a8 | 37 | float getRadian(float frequency) |
sgrsn | 1:f102831401a8 | 38 | { |
sgrsn | 1:f102831401a8 | 39 | last_time = current_time; |
sgrsn | 1:f102831401a8 | 40 | current_time = _t -> read(); |
sgrsn | 1:f102831401a8 | 41 | float dt = current_time - last_time; |
sgrsn | 1:f102831401a8 | 42 | float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 43 | return delta_rad; |
sgrsn | 1:f102831401a8 | 44 | } |
sgrsn | 1:f102831401a8 | 45 | |
sgrsn | 1:f102831401a8 | 46 | private: |
sgrsn | 1:f102831401a8 | 47 | Timer *_t; |
sgrsn | 1:f102831401a8 | 48 | float last_time; |
sgrsn | 1:f102831401a8 | 49 | float current_time; |
sgrsn | 1:f102831401a8 | 50 | }; |
sgrsn | 1:f102831401a8 | 51 | |
sgrsn | 1:f102831401a8 | 52 | class MakePath |
sgrsn | 1:f102831401a8 | 53 | { |
sgrsn | 1:f102831401a8 | 54 | public: |
sgrsn | 1:f102831401a8 | 55 | MakePath() |
sgrsn | 1:f102831401a8 | 56 | { |
sgrsn | 1:f102831401a8 | 57 | } |
sgrsn | 1:f102831401a8 | 58 | int makePath(int targetX, int targetY, int targetZ, int x, int y, int z) |
sgrsn | 1:f102831401a8 | 59 | { |
sgrsn | 1:f102831401a8 | 60 | int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0; //5mm間隔 |
sgrsn | 1:f102831401a8 | 61 | //printf("num = %d\r\n", num); |
sgrsn | 1:f102831401a8 | 62 | for(int i = 1; i <= num; i++) |
sgrsn | 1:f102831401a8 | 63 | { |
sgrsn | 1:f102831401a8 | 64 | float a = float(i) / num; |
sgrsn | 1:f102831401a8 | 65 | PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); |
sgrsn | 1:f102831401a8 | 66 | PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); |
sgrsn | 1:f102831401a8 | 67 | PATH[i-1][2] = z + float(targetZ - z) * a; |
sgrsn | 1:f102831401a8 | 68 | } |
sgrsn | 1:f102831401a8 | 69 | if(num > 0) |
sgrsn | 1:f102831401a8 | 70 | { |
sgrsn | 1:f102831401a8 | 71 | PATH[num-1][0] = targetX; |
sgrsn | 1:f102831401a8 | 72 | PATH[num-1][1] = targetY; |
sgrsn | 1:f102831401a8 | 73 | PATH[num-1][2] = targetZ; |
sgrsn | 1:f102831401a8 | 74 | } |
sgrsn | 1:f102831401a8 | 75 | else |
sgrsn | 1:f102831401a8 | 76 | { |
sgrsn | 1:f102831401a8 | 77 | PATH[0][0] = targetX; |
sgrsn | 1:f102831401a8 | 78 | PATH[0][1] = targetY; |
sgrsn | 1:f102831401a8 | 79 | PATH[0][2] = targetZ; |
sgrsn | 1:f102831401a8 | 80 | num = 1; |
sgrsn | 1:f102831401a8 | 81 | } |
sgrsn | 1:f102831401a8 | 82 | return num; |
sgrsn | 1:f102831401a8 | 83 | } |
sgrsn | 1:f102831401a8 | 84 | |
sgrsn | 1:f102831401a8 | 85 | int getPathX(int i) |
sgrsn | 1:f102831401a8 | 86 | { |
sgrsn | 1:f102831401a8 | 87 | return PATH[i][0]; |
sgrsn | 1:f102831401a8 | 88 | } |
sgrsn | 1:f102831401a8 | 89 | int getPathY(int i) |
sgrsn | 1:f102831401a8 | 90 | { |
sgrsn | 1:f102831401a8 | 91 | return PATH[i][1]; |
sgrsn | 1:f102831401a8 | 92 | } |
sgrsn | 1:f102831401a8 | 93 | int getPathZ(int i) |
sgrsn | 1:f102831401a8 | 94 | { |
sgrsn | 1:f102831401a8 | 95 | return PATH[i][2]; |
sgrsn | 1:f102831401a8 | 96 | } |
sgrsn | 1:f102831401a8 | 97 | |
sgrsn | 1:f102831401a8 | 98 | private: |
sgrsn | 1:f102831401a8 | 99 | int PATH[500][3]; |
sgrsn | 1:f102831401a8 | 100 | }; |
sgrsn | 0:f1459eec7228 | 101 | |
sgrsn | 0:f1459eec7228 | 102 | int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; |
sgrsn | 0:f1459eec7228 | 103 | int Register[0x20] = {}; |
sgrsn | 0:f1459eec7228 | 104 | |
sgrsn | 0:f1459eec7228 | 105 | Serial PC(USBTX, USBRX); |
sgrsn | 0:f1459eec7228 | 106 | i2c master(p28, p27); |
sgrsn | 0:f1459eec7228 | 107 | BusOut LEDs(LED1, LED2, LED3, LED4); |
sgrsn | 0:f1459eec7228 | 108 | Timer timer; |
sgrsn | 0:f1459eec7228 | 109 | JY901 jy901(&master, &timer); |
sgrsn | 1:f102831401a8 | 110 | MakePath myPath; |
sgrsn | 0:f1459eec7228 | 111 | |
sgrsn | 0:f1459eec7228 | 112 | void controlFromGcode() |
sgrsn | 0:f1459eec7228 | 113 | { |
sgrsn | 1:f102831401a8 | 114 | float threshold_x = 5; //[mm] |
sgrsn | 1:f102831401a8 | 115 | float threshold_y = 5; //[mm] |
sgrsn | 0:f1459eec7228 | 116 | float threshold_theta = 5 * DEG_TO_RAD; |
sgrsn | 0:f1459eec7228 | 117 | |
sgrsn | 0:f1459eec7228 | 118 | // 角度補正係数 |
sgrsn | 0:f1459eec7228 | 119 | float L = 233; //[mm] |
sgrsn | 0:f1459eec7228 | 120 | |
sgrsn | 0:f1459eec7228 | 121 | Timer timer2; |
sgrsn | 0:f1459eec7228 | 122 | PID pid_x(&timer2); |
sgrsn | 0:f1459eec7228 | 123 | PID pid_y(&timer2); |
sgrsn | 0:f1459eec7228 | 124 | PID pid_theta(&timer2); |
sgrsn | 1:f102831401a8 | 125 | pid_x.setParameter(200.0, 0.0, 0.0); |
sgrsn | 1:f102831401a8 | 126 | pid_y.setParameter(200.0, 0.0, 0.0); |
sgrsn | 1:f102831401a8 | 127 | pid_theta.setParameter(100.0, 0.0, 0.0); |
sgrsn | 0:f1459eec7228 | 128 | |
sgrsn | 0:f1459eec7228 | 129 | // Gコード読み取り |
sgrsn | 0:f1459eec7228 | 130 | LocalFileSystem local("local"); |
sgrsn | 0:f1459eec7228 | 131 | int array[SIZE] = {}; |
sgrsn | 0:f1459eec7228 | 132 | FILE *fp = fopen( "/local/guide1.txt", "r"); |
sgrsn | 0:f1459eec7228 | 133 | MakeSequencer code(fp); |
sgrsn | 0:f1459eec7228 | 134 | |
sgrsn | 0:f1459eec7228 | 135 | int row = 1; |
sgrsn | 0:f1459eec7228 | 136 | float v[4] = {}; |
sgrsn | 0:f1459eec7228 | 137 | |
sgrsn | 1:f102831401a8 | 138 | Timer temp_t; |
sgrsn | 1:f102831401a8 | 139 | CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)}; |
sgrsn | 1:f102831401a8 | 140 | float wheel_rad[4] = {}; |
sgrsn | 1:f102831401a8 | 141 | |
sgrsn | 1:f102831401a8 | 142 | //TextLCD lcd(p24, p26, p27, p28, p29, p30); |
sgrsn | 1:f102831401a8 | 143 | |
sgrsn | 1:f102831401a8 | 144 | float x_robot = 0; |
sgrsn | 1:f102831401a8 | 145 | float y_robot = 0; |
sgrsn | 1:f102831401a8 | 146 | float theta_robot = 0; |
sgrsn | 1:f102831401a8 | 147 | |
sgrsn | 1:f102831401a8 | 148 | |
sgrsn | 1:f102831401a8 | 149 | // 目標位置把握 |
sgrsn | 1:f102831401a8 | 150 | code.getGcode(row,sizeof(array)/sizeof(int),array); |
sgrsn | 1:f102831401a8 | 151 | float x_desire = array[0]; |
sgrsn | 1:f102831401a8 | 152 | float y_desire = array[1]; |
sgrsn | 1:f102831401a8 | 153 | float theta_desire = float(array[2]) *DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 154 | int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot); |
sgrsn | 1:f102831401a8 | 155 | |
sgrsn | 1:f102831401a8 | 156 | int path = 0; |
sgrsn | 0:f1459eec7228 | 157 | |
sgrsn | 0:f1459eec7228 | 158 | while(1) |
sgrsn | 0:f1459eec7228 | 159 | { |
sgrsn | 0:f1459eec7228 | 160 | |
sgrsn | 0:f1459eec7228 | 161 | // 自己位置推定 |
sgrsn | 1:f102831401a8 | 162 | //float x_robot = Register[MARKER_X]; |
sgrsn | 1:f102831401a8 | 163 | //float y_robot = Register[MARKER_Y]; |
sgrsn | 1:f102831401a8 | 164 | //float theta_robot = float(Register[MARKER_YAW]) / 1000.0; |
sgrsn | 1:f102831401a8 | 165 | theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 166 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 1:f102831401a8 | 167 | { |
sgrsn | 1:f102831401a8 | 168 | wheel_rad[i] = counter[i].getRadian(v[i]); |
sgrsn | 1:f102831401a8 | 169 | } |
sgrsn | 1:f102831401a8 | 170 | float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.7; |
sgrsn | 1:f102831401a8 | 171 | float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.7; |
sgrsn | 0:f1459eec7228 | 172 | |
sgrsn | 1:f102831401a8 | 173 | x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot); |
sgrsn | 1:f102831401a8 | 174 | y_robot += dy * cos(theta_robot) + dx * sin(theta_robot); |
sgrsn | 0:f1459eec7228 | 175 | |
sgrsn | 0:f1459eec7228 | 176 | // 目標位置判定 |
sgrsn | 0:f1459eec7228 | 177 | float err_x = x_desire - x_robot; |
sgrsn | 0:f1459eec7228 | 178 | float err_y = y_desire - y_robot; |
sgrsn | 0:f1459eec7228 | 179 | float err_theta = theta_desire - theta_robot; |
sgrsn | 0:f1459eec7228 | 180 | |
sgrsn | 1:f102831401a8 | 181 | //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row); |
sgrsn | 1:f102831401a8 | 182 | printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row); |
sgrsn | 1:f102831401a8 | 183 | //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row); |
sgrsn | 1:f102831401a8 | 184 | |
sgrsn | 0:f1459eec7228 | 185 | // 目標位置到達 |
sgrsn | 0:f1459eec7228 | 186 | if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) |
sgrsn | 0:f1459eec7228 | 187 | { |
sgrsn | 0:f1459eec7228 | 188 | // 車輪を停止 |
sgrsn | 0:f1459eec7228 | 189 | coastAllMotor(); |
sgrsn | 1:f102831401a8 | 190 | //pid_x.reset(); |
sgrsn | 1:f102831401a8 | 191 | //pid_y.reset(); |
sgrsn | 1:f102831401a8 | 192 | //pid_theta.reset(); |
sgrsn | 1:f102831401a8 | 193 | wait_ms(500); |
sgrsn | 1:f102831401a8 | 194 | jy901.reset(); |
sgrsn | 0:f1459eec7228 | 195 | |
sgrsn | 0:f1459eec7228 | 196 | // Gコードを次の行に |
sgrsn | 0:f1459eec7228 | 197 | row++; |
sgrsn | 1:f102831401a8 | 198 | if(row > code.getGcodeSize()) |
sgrsn | 1:f102831401a8 | 199 | { |
sgrsn | 1:f102831401a8 | 200 | row = 0; |
sgrsn | 1:f102831401a8 | 201 | } |
sgrsn | 1:f102831401a8 | 202 | |
sgrsn | 1:f102831401a8 | 203 | // 目標位置把握 |
sgrsn | 0:f1459eec7228 | 204 | code.getGcode(row, ArraySize(array), array); |
sgrsn | 1:f102831401a8 | 205 | x_desire = array[0]; |
sgrsn | 1:f102831401a8 | 206 | y_desire = array[1]; |
sgrsn | 1:f102831401a8 | 207 | theta_desire = float(array[2]) *DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 208 | pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG); |
sgrsn | 1:f102831401a8 | 209 | path = 0; |
sgrsn | 0:f1459eec7228 | 210 | } |
sgrsn | 0:f1459eec7228 | 211 | |
sgrsn | 0:f1459eec7228 | 212 | // 目標速度計算 |
sgrsn | 0:f1459eec7228 | 213 | else |
sgrsn | 0:f1459eec7228 | 214 | { |
sgrsn | 0:f1459eec7228 | 215 | // 慣性座標での速度 |
sgrsn | 1:f102831401a8 | 216 | float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); |
sgrsn | 1:f102831401a8 | 217 | float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); |
sgrsn | 1:f102831401a8 | 218 | float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); |
sgrsn | 1:f102831401a8 | 219 | path++; |
sgrsn | 1:f102831401a8 | 220 | if(path >= pathSize) path = pathSize-1; |
sgrsn | 0:f1459eec7228 | 221 | |
sgrsn | 0:f1459eec7228 | 222 | // ロボット座標での速度 |
sgrsn | 0:f1459eec7228 | 223 | float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; |
sgrsn | 0:f1459eec7228 | 224 | float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; |
sgrsn | 0:f1459eec7228 | 225 | |
sgrsn | 0:f1459eec7228 | 226 | // 各車輪の速度 |
sgrsn | 0:f1459eec7228 | 227 | v[0] = -x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 228 | v[1] = x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 229 | v[2] = x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 230 | v[3] = -x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 231 | |
sgrsn | 0:f1459eec7228 | 232 | // 本当はこうするべき |
sgrsn | 0:f1459eec7228 | 233 | // f = v * ppr / ( 2*PI * r); |
sgrsn | 0:f1459eec7228 | 234 | |
sgrsn | 0:f1459eec7228 | 235 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:f1459eec7228 | 236 | { |
sgrsn | 1:f102831401a8 | 237 | controlMotor(i, (int)v[i]); |
sgrsn | 0:f1459eec7228 | 238 | } |
sgrsn | 0:f1459eec7228 | 239 | } |
sgrsn | 0:f1459eec7228 | 240 | } |
sgrsn | 0:f1459eec7228 | 241 | } |
sgrsn | 0:f1459eec7228 | 242 | |
sgrsn | 0:f1459eec7228 | 243 | int main() |
sgrsn | 0:f1459eec7228 | 244 | { |
sgrsn | 0:f1459eec7228 | 245 | coastAllMotor(); |
sgrsn | 0:f1459eec7228 | 246 | PC.baud(9600); |
sgrsn | 1:f102831401a8 | 247 | //PC.attach(serialRead); |
sgrsn | 1:f102831401a8 | 248 | jy901.calibrateAll(5000); |
sgrsn | 1:f102831401a8 | 249 | //controlFromWASD(); |
sgrsn | 1:f102831401a8 | 250 | PC.printf("\r\nI'm ready to start. Press Enter\r\n"); |
sgrsn | 1:f102831401a8 | 251 | bool startable = false; |
sgrsn | 1:f102831401a8 | 252 | while(!startable) |
sgrsn | 1:f102831401a8 | 253 | { |
sgrsn | 1:f102831401a8 | 254 | startable = PC.readable() > 0; |
sgrsn | 1:f102831401a8 | 255 | } |
sgrsn | 0:f1459eec7228 | 256 | controlFromGcode(); |
sgrsn | 0:f1459eec7228 | 257 | } |
sgrsn | 0:f1459eec7228 | 258 | |
sgrsn | 1:f102831401a8 | 259 | int controlMotor(int ch, int frequency) |
sgrsn | 0:f1459eec7228 | 260 | { |
sgrsn | 0:f1459eec7228 | 261 | int dir = COAST; |
sgrsn | 0:f1459eec7228 | 262 | int size = 4; |
sgrsn | 0:f1459eec7228 | 263 | if(ch < 0 || ch > 3) |
sgrsn | 0:f1459eec7228 | 264 | { |
sgrsn | 0:f1459eec7228 | 265 | //channel error |
sgrsn | 1:f102831401a8 | 266 | return 0; |
sgrsn | 0:f1459eec7228 | 267 | } |
sgrsn | 0:f1459eec7228 | 268 | else |
sgrsn | 0:f1459eec7228 | 269 | { |
sgrsn | 0:f1459eec7228 | 270 | if(frequency > 0) |
sgrsn | 0:f1459eec7228 | 271 | { |
sgrsn | 0:f1459eec7228 | 272 | dir = CW; |
sgrsn | 0:f1459eec7228 | 273 | } |
sgrsn | 0:f1459eec7228 | 274 | else if(frequency < 0) |
sgrsn | 0:f1459eec7228 | 275 | { |
sgrsn | 0:f1459eec7228 | 276 | dir = CCW; |
sgrsn | 0:f1459eec7228 | 277 | frequency = -frequency; |
sgrsn | 0:f1459eec7228 | 278 | } |
sgrsn | 0:f1459eec7228 | 279 | else |
sgrsn | 0:f1459eec7228 | 280 | { |
sgrsn | 0:f1459eec7228 | 281 | dir = BRAKE; |
sgrsn | 0:f1459eec7228 | 282 | } |
sgrsn | 0:f1459eec7228 | 283 | // 周波数制限 脱調を防ぐ |
sgrsn | 0:f1459eec7228 | 284 | if(frequency > MaxFrequency) frequency = MaxFrequency; |
sgrsn | 1:f102831401a8 | 285 | //else if(frequency < MinFrequency) frequency = MinFrequency; |
sgrsn | 0:f1459eec7228 | 286 | |
sgrsn | 0:f1459eec7228 | 287 | master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); |
sgrsn | 0:f1459eec7228 | 288 | master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); |
sgrsn | 1:f102831401a8 | 289 | |
sgrsn | 1:f102831401a8 | 290 | return frequency; |
sgrsn | 0:f1459eec7228 | 291 | } |
sgrsn | 0:f1459eec7228 | 292 | } |
sgrsn | 0:f1459eec7228 | 293 | |
sgrsn | 0:f1459eec7228 | 294 | |
sgrsn | 0:f1459eec7228 | 295 | void coastAllMotor() |
sgrsn | 0:f1459eec7228 | 296 | { |
sgrsn | 0:f1459eec7228 | 297 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:f1459eec7228 | 298 | { |
sgrsn | 0:f1459eec7228 | 299 | master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4); |
sgrsn | 0:f1459eec7228 | 300 | } |
sgrsn | 0:f1459eec7228 | 301 | } |
sgrsn | 0:f1459eec7228 | 302 | |
sgrsn | 0:f1459eec7228 | 303 | void serialRead() |
sgrsn | 0:f1459eec7228 | 304 | { |
sgrsn | 0:f1459eec7228 | 305 | int reg = 0; |
sgrsn | 0:f1459eec7228 | 306 | uint8_t data[4] = {}; |
sgrsn | 0:f1459eec7228 | 307 | if(PC.readable() > 0) |
sgrsn | 0:f1459eec7228 | 308 | { |
sgrsn | 0:f1459eec7228 | 309 | reg = PC.getc(); |
sgrsn | 0:f1459eec7228 | 310 | data[0] = PC.getc(); |
sgrsn | 0:f1459eec7228 | 311 | data[1] = PC.getc(); |
sgrsn | 0:f1459eec7228 | 312 | data[2] = PC.getc(); |
sgrsn | 0:f1459eec7228 | 313 | data[3] = PC.getc(); |
sgrsn | 0:f1459eec7228 | 314 | } |
sgrsn | 0:f1459eec7228 | 315 | Register[reg % 0x20] = 0; |
sgrsn | 0:f1459eec7228 | 316 | for(int i = 0; i < 4; i++) |
sgrsn | 0:f1459eec7228 | 317 | Register[reg % 0x20] |= int(data[i]) << (i*8); |
sgrsn | 0:f1459eec7228 | 318 | } |
sgrsn | 0:f1459eec7228 | 319 | |
sgrsn | 0:f1459eec7228 | 320 | |
sgrsn | 0:f1459eec7228 | 321 | |
sgrsn | 0:f1459eec7228 | 322 | /*Function for Test***********************************************************/ |
sgrsn | 0:f1459eec7228 | 323 | |
sgrsn | 0:f1459eec7228 | 324 | void controlFrequencyFromTerminal() |
sgrsn | 0:f1459eec7228 | 325 | { |
sgrsn | 0:f1459eec7228 | 326 | int ch, speed; |
sgrsn | 0:f1459eec7228 | 327 | if(PC.readable() > 0) |
sgrsn | 0:f1459eec7228 | 328 | { |
sgrsn | 0:f1459eec7228 | 329 | PC.scanf("M%d:%d", &ch, &speed); |
sgrsn | 0:f1459eec7228 | 330 | PC.printf("M%d:%d\r\n", ch, speed); |
sgrsn | 0:f1459eec7228 | 331 | if(ch < 0 || ch > 3) |
sgrsn | 0:f1459eec7228 | 332 | PC.printf("channnel error\r\n"); |
sgrsn | 0:f1459eec7228 | 333 | else |
sgrsn | 0:f1459eec7228 | 334 | { |
sgrsn | 0:f1459eec7228 | 335 | controlMotor(ch, speed); |
sgrsn | 0:f1459eec7228 | 336 | } |
sgrsn | 0:f1459eec7228 | 337 | } |
sgrsn | 0:f1459eec7228 | 338 | } |
sgrsn | 0:f1459eec7228 | 339 | |
sgrsn | 0:f1459eec7228 | 340 | void controlFromWASD() |
sgrsn | 0:f1459eec7228 | 341 | { |
sgrsn | 0:f1459eec7228 | 342 | float L = 4.0; |
sgrsn | 0:f1459eec7228 | 343 | float v[4] = {}; |
sgrsn | 0:f1459eec7228 | 344 | char input = 0; |
sgrsn | 0:f1459eec7228 | 345 | while(1) |
sgrsn | 0:f1459eec7228 | 346 | { |
sgrsn | 0:f1459eec7228 | 347 | if(PC.readable() > 0) |
sgrsn | 0:f1459eec7228 | 348 | { |
sgrsn | 0:f1459eec7228 | 349 | input = PC.getc(); |
sgrsn | 0:f1459eec7228 | 350 | //printf("%c\r\n", input); |
sgrsn | 0:f1459eec7228 | 351 | } |
sgrsn | 0:f1459eec7228 | 352 | |
sgrsn | 0:f1459eec7228 | 353 | float xI_dot = 0; |
sgrsn | 0:f1459eec7228 | 354 | float yI_dot = 0; |
sgrsn | 0:f1459eec7228 | 355 | switch(input) |
sgrsn | 0:f1459eec7228 | 356 | { |
sgrsn | 0:f1459eec7228 | 357 | case 'a': |
sgrsn | 0:f1459eec7228 | 358 | xI_dot = -1; |
sgrsn | 0:f1459eec7228 | 359 | yI_dot = 0; |
sgrsn | 0:f1459eec7228 | 360 | break; |
sgrsn | 0:f1459eec7228 | 361 | case 'd': |
sgrsn | 0:f1459eec7228 | 362 | xI_dot = 1; |
sgrsn | 0:f1459eec7228 | 363 | yI_dot = 0; |
sgrsn | 0:f1459eec7228 | 364 | break; |
sgrsn | 0:f1459eec7228 | 365 | case 'w': |
sgrsn | 0:f1459eec7228 | 366 | xI_dot = 0; |
sgrsn | 0:f1459eec7228 | 367 | yI_dot = 1; |
sgrsn | 0:f1459eec7228 | 368 | break; |
sgrsn | 0:f1459eec7228 | 369 | case 's': |
sgrsn | 0:f1459eec7228 | 370 | xI_dot = 0; |
sgrsn | 0:f1459eec7228 | 371 | yI_dot = -1; |
sgrsn | 0:f1459eec7228 | 372 | break; |
sgrsn | 0:f1459eec7228 | 373 | case ' ': |
sgrsn | 0:f1459eec7228 | 374 | xI_dot = 0; |
sgrsn | 0:f1459eec7228 | 375 | yI_dot = 0; |
sgrsn | 0:f1459eec7228 | 376 | break; |
sgrsn | 0:f1459eec7228 | 377 | } |
sgrsn | 0:f1459eec7228 | 378 | //master.getSlaveRegistarData(addr, 1, &data, size); |
sgrsn | 0:f1459eec7228 | 379 | |
sgrsn | 0:f1459eec7228 | 380 | float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; |
sgrsn | 0:f1459eec7228 | 381 | |
sgrsn | 0:f1459eec7228 | 382 | float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot; |
sgrsn | 0:f1459eec7228 | 383 | float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot; |
sgrsn | 0:f1459eec7228 | 384 | float theta_dot = 0.0 - theta_z; |
sgrsn | 0:f1459eec7228 | 385 | |
sgrsn | 0:f1459eec7228 | 386 | v[1] = x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 387 | v[2] = x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 388 | v[3] = -x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 389 | v[0] = -x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 390 | |
sgrsn | 0:f1459eec7228 | 391 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:f1459eec7228 | 392 | { |
sgrsn | 0:f1459eec7228 | 393 | controlMotor(i, v[i] * 20000.0); |
sgrsn | 0:f1459eec7228 | 394 | } |
sgrsn | 0:f1459eec7228 | 395 | |
sgrsn | 0:f1459eec7228 | 396 | PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]); |
sgrsn | 0:f1459eec7228 | 397 | |
sgrsn | 0:f1459eec7228 | 398 | //PC.printf("%f\r\n", theta_z); |
sgrsn | 0:f1459eec7228 | 399 | } |
sgrsn | 0:f1459eec7228 | 400 | } |