hidaka sato / Mbed 2 deprecated WRS2019_master

WRS2019

Dependencies:   mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3

main.cpp@2:83fa142c5bcc, 2019-12-17 (annotated)

Committer:
sgrsn
Date:
Tue Dec 17 07:23:55 2019 +0000
Revision:
2:83fa142c5bcc
Parent:
1:f102831401a8
Child:
3:4ac32aff309c
Fix start command

Who changed what in which revision?

UserRevisionLine numberNew 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 2:83fa142c5bcc 116 float threshold_theta = 1 * 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 2:83fa142c5bcc 170 float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
sgrsn 2:83fa142c5bcc 171 float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
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 2:83fa142c5bcc 251 //bool startable = false;
sgrsn 2:83fa142c5bcc 252 char input_character = 0;
sgrsn 2:83fa142c5bcc 253 while(input_character != 'a')
sgrsn 1:f102831401a8 254 {
sgrsn 2:83fa142c5bcc 255 if(PC.readable() > 0)
sgrsn 2:83fa142c5bcc 256 {
sgrsn 2:83fa142c5bcc 257 input_character = PC.getc();
sgrsn 2:83fa142c5bcc 258 }
sgrsn 1:f102831401a8 259 }
sgrsn 0:f1459eec7228 260 controlFromGcode();
sgrsn 0:f1459eec7228 261 }
sgrsn 0:f1459eec7228 262
sgrsn 1:f102831401a8 263 int controlMotor(int ch, int frequency)
sgrsn 0:f1459eec7228 264 {
sgrsn 0:f1459eec7228 265 int dir = COAST;
sgrsn 0:f1459eec7228 266 int size = 4;
sgrsn 0:f1459eec7228 267 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 268 {
sgrsn 0:f1459eec7228 269 //channel error
sgrsn 1:f102831401a8 270 return 0;
sgrsn 0:f1459eec7228 271 }
sgrsn 0:f1459eec7228 272 else
sgrsn 0:f1459eec7228 273 {
sgrsn 0:f1459eec7228 274 if(frequency > 0)
sgrsn 0:f1459eec7228 275 {
sgrsn 0:f1459eec7228 276 dir = CW;
sgrsn 0:f1459eec7228 277 }
sgrsn 0:f1459eec7228 278 else if(frequency < 0)
sgrsn 0:f1459eec7228 279 {
sgrsn 0:f1459eec7228 280 dir = CCW;
sgrsn 0:f1459eec7228 281 frequency = -frequency;
sgrsn 0:f1459eec7228 282 }
sgrsn 0:f1459eec7228 283 else
sgrsn 0:f1459eec7228 284 {
sgrsn 0:f1459eec7228 285 dir = BRAKE;
sgrsn 0:f1459eec7228 286 }
sgrsn 0:f1459eec7228 287 // 周波数制限 脱調を防ぐ
sgrsn 0:f1459eec7228 288 if(frequency > MaxFrequency) frequency = MaxFrequency;
sgrsn 1:f102831401a8 289 //else if(frequency < MinFrequency) frequency = MinFrequency;
sgrsn 0:f1459eec7228 290
sgrsn 0:f1459eec7228 291 master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
sgrsn 0:f1459eec7228 292 master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
sgrsn 1:f102831401a8 293
sgrsn 1:f102831401a8 294 return frequency;
sgrsn 0:f1459eec7228 295 }
sgrsn 0:f1459eec7228 296 }
sgrsn 0:f1459eec7228 297
sgrsn 0:f1459eec7228 298
sgrsn 0:f1459eec7228 299 void coastAllMotor()
sgrsn 0:f1459eec7228 300 {
sgrsn 0:f1459eec7228 301 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 302 {
sgrsn 0:f1459eec7228 303 master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
sgrsn 0:f1459eec7228 304 }
sgrsn 0:f1459eec7228 305 }
sgrsn 0:f1459eec7228 306
sgrsn 0:f1459eec7228 307 void serialRead()
sgrsn 0:f1459eec7228 308 {
sgrsn 0:f1459eec7228 309 int reg = 0;
sgrsn 0:f1459eec7228 310 uint8_t data[4] = {};
sgrsn 0:f1459eec7228 311 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 312 {
sgrsn 0:f1459eec7228 313 reg = PC.getc();
sgrsn 0:f1459eec7228 314 data[0] = PC.getc();
sgrsn 0:f1459eec7228 315 data[1] = PC.getc();
sgrsn 0:f1459eec7228 316 data[2] = PC.getc();
sgrsn 0:f1459eec7228 317 data[3] = PC.getc();
sgrsn 0:f1459eec7228 318 }
sgrsn 0:f1459eec7228 319 Register[reg % 0x20] = 0;
sgrsn 0:f1459eec7228 320 for(int i = 0; i < 4; i++)
sgrsn 0:f1459eec7228 321 Register[reg % 0x20] |= int(data[i]) << (i*8);
sgrsn 0:f1459eec7228 322 }
sgrsn 0:f1459eec7228 323
sgrsn 0:f1459eec7228 324
sgrsn 0:f1459eec7228 325
sgrsn 0:f1459eec7228 326 /*Function for Test***********************************************************/
sgrsn 0:f1459eec7228 327
sgrsn 0:f1459eec7228 328 void controlFrequencyFromTerminal()
sgrsn 0:f1459eec7228 329 {
sgrsn 0:f1459eec7228 330 int ch, speed;
sgrsn 0:f1459eec7228 331 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 332 {
sgrsn 0:f1459eec7228 333 PC.scanf("M%d:%d", &ch, &speed);
sgrsn 0:f1459eec7228 334 PC.printf("M%d:%d\r\n", ch, speed);
sgrsn 0:f1459eec7228 335 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 336 PC.printf("channnel error\r\n");
sgrsn 0:f1459eec7228 337 else
sgrsn 0:f1459eec7228 338 {
sgrsn 0:f1459eec7228 339 controlMotor(ch, speed);
sgrsn 0:f1459eec7228 340 }
sgrsn 0:f1459eec7228 341 }
sgrsn 0:f1459eec7228 342 }
sgrsn 0:f1459eec7228 343
sgrsn 0:f1459eec7228 344 void controlFromWASD()
sgrsn 0:f1459eec7228 345 {
sgrsn 0:f1459eec7228 346 float L = 4.0;
sgrsn 0:f1459eec7228 347 float v[4] = {};
sgrsn 0:f1459eec7228 348 char input = 0;
sgrsn 0:f1459eec7228 349 while(1)
sgrsn 0:f1459eec7228 350 {
sgrsn 0:f1459eec7228 351 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 352 {
sgrsn 0:f1459eec7228 353 input = PC.getc();
sgrsn 0:f1459eec7228 354 //printf("%c\r\n", input);
sgrsn 0:f1459eec7228 355 }
sgrsn 0:f1459eec7228 356
sgrsn 0:f1459eec7228 357 float xI_dot = 0;
sgrsn 0:f1459eec7228 358 float yI_dot = 0;
sgrsn 0:f1459eec7228 359 switch(input)
sgrsn 0:f1459eec7228 360 {
sgrsn 0:f1459eec7228 361 case 'a':
sgrsn 0:f1459eec7228 362 xI_dot = -1;
sgrsn 0:f1459eec7228 363 yI_dot = 0;
sgrsn 0:f1459eec7228 364 break;
sgrsn 0:f1459eec7228 365 case 'd':
sgrsn 0:f1459eec7228 366 xI_dot = 1;
sgrsn 0:f1459eec7228 367 yI_dot = 0;
sgrsn 0:f1459eec7228 368 break;
sgrsn 0:f1459eec7228 369 case 'w':
sgrsn 0:f1459eec7228 370 xI_dot = 0;
sgrsn 0:f1459eec7228 371 yI_dot = 1;
sgrsn 0:f1459eec7228 372 break;
sgrsn 0:f1459eec7228 373 case 's':
sgrsn 0:f1459eec7228 374 xI_dot = 0;
sgrsn 0:f1459eec7228 375 yI_dot = -1;
sgrsn 0:f1459eec7228 376 break;
sgrsn 0:f1459eec7228 377 case ' ':
sgrsn 0:f1459eec7228 378 xI_dot = 0;
sgrsn 0:f1459eec7228 379 yI_dot = 0;
sgrsn 0:f1459eec7228 380 break;
sgrsn 0:f1459eec7228 381 }
sgrsn 0:f1459eec7228 382 //master.getSlaveRegistarData(addr, 1, &data, size);
sgrsn 0:f1459eec7228 383
sgrsn 0:f1459eec7228 384 float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 0:f1459eec7228 385
sgrsn 0:f1459eec7228 386 float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 387 float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 388 float theta_dot = 0.0 - theta_z;
sgrsn 0:f1459eec7228 389
sgrsn 0:f1459eec7228 390 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 391 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 392 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 393 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 394
sgrsn 0:f1459eec7228 395 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 396 {
sgrsn 0:f1459eec7228 397 controlMotor(i, v[i] * 20000.0);
sgrsn 0:f1459eec7228 398 }
sgrsn 0:f1459eec7228 399
sgrsn 0:f1459eec7228 400 PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]);
sgrsn 0:f1459eec7228 401
sgrsn 0:f1459eec7228 402 //PC.printf("%f\r\n", theta_z);
sgrsn 0:f1459eec7228 403 }
sgrsn 0:f1459eec7228 404 }