hidaka sato / Mbed 2 deprecated WRS2019_master

WRS2019

Dependencies:   mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3

main.cpp@3:4ac32aff309c, 2019-12-17 (annotated)

Committer:
sgrsn
Date:
Tue Dec 17 12:42:15 2019 +0000
Revision:
3:4ac32aff309c
Parent:
2:83fa142c5bcc
Child:
4:25ab7216447f
After Finish task, Robot don't loop task.

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 3:4ac32aff309c 134 printf("size:%d\r\n", code.getGcodeSize());
sgrsn 3:4ac32aff309c 135 for(int i = 0; i < code.getGcodeSize(); i++)
sgrsn 3:4ac32aff309c 136 {
sgrsn 3:4ac32aff309c 137 code.getGcode(i,sizeof(array)/sizeof(int),array);
sgrsn 3:4ac32aff309c 138 printf("%d, %d, %d\r\n", array[0], array[1], i);
sgrsn 3:4ac32aff309c 139 }
sgrsn 0:f1459eec7228 140
sgrsn 0:f1459eec7228 141 int row = 1;
sgrsn 0:f1459eec7228 142 float v[4] = {};
sgrsn 0:f1459eec7228 143
sgrsn 1:f102831401a8 144 Timer temp_t;
sgrsn 1:f102831401a8 145 CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)};
sgrsn 1:f102831401a8 146 float wheel_rad[4] = {};
sgrsn 1:f102831401a8 147
sgrsn 1:f102831401a8 148 //TextLCD lcd(p24, p26, p27, p28, p29, p30);
sgrsn 1:f102831401a8 149
sgrsn 1:f102831401a8 150 float x_robot = 0;
sgrsn 1:f102831401a8 151 float y_robot = 0;
sgrsn 1:f102831401a8 152 float theta_robot = 0;
sgrsn 1:f102831401a8 153
sgrsn 1:f102831401a8 154
sgrsn 1:f102831401a8 155 // 目標位置把握
sgrsn 1:f102831401a8 156 code.getGcode(row,sizeof(array)/sizeof(int),array);
sgrsn 1:f102831401a8 157 float x_desire = array[0];
sgrsn 1:f102831401a8 158 float y_desire = array[1];
sgrsn 1:f102831401a8 159 float theta_desire = float(array[2]) *DEG_TO_RAD;
sgrsn 1:f102831401a8 160 int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot);
sgrsn 1:f102831401a8 161
sgrsn 1:f102831401a8 162 int path = 0;
sgrsn 0:f1459eec7228 163
sgrsn 0:f1459eec7228 164 while(1)
sgrsn 0:f1459eec7228 165 {
sgrsn 0:f1459eec7228 166
sgrsn 0:f1459eec7228 167 // 自己位置推定
sgrsn 1:f102831401a8 168 //float x_robot = Register[MARKER_X];
sgrsn 1:f102831401a8 169 //float y_robot = Register[MARKER_Y];
sgrsn 1:f102831401a8 170 //float theta_robot = float(Register[MARKER_YAW]) / 1000.0;
sgrsn 1:f102831401a8 171 theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 1:f102831401a8 172 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 1:f102831401a8 173 {
sgrsn 1:f102831401a8 174 wheel_rad[i] = counter[i].getRadian(v[i]);
sgrsn 1:f102831401a8 175 }
sgrsn 2:83fa142c5bcc 176 float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
sgrsn 2:83fa142c5bcc 177 float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
sgrsn 0:f1459eec7228 178
sgrsn 1:f102831401a8 179 x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot);
sgrsn 1:f102831401a8 180 y_robot += dy * cos(theta_robot) + dx * sin(theta_robot);
sgrsn 0:f1459eec7228 181
sgrsn 0:f1459eec7228 182 // 目標位置判定
sgrsn 0:f1459eec7228 183 float err_x = x_desire - x_robot;
sgrsn 0:f1459eec7228 184 float err_y = y_desire - y_robot;
sgrsn 0:f1459eec7228 185 float err_theta = theta_desire - theta_robot;
sgrsn 0:f1459eec7228 186
sgrsn 1:f102831401a8 187 //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row);
sgrsn 1:f102831401a8 188 printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row);
sgrsn 1:f102831401a8 189 //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row);
sgrsn 1:f102831401a8 190
sgrsn 0:f1459eec7228 191 // 目標位置到達
sgrsn 0:f1459eec7228 192 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
sgrsn 0:f1459eec7228 193 {
sgrsn 0:f1459eec7228 194 // 車輪を停止
sgrsn 0:f1459eec7228 195 coastAllMotor();
sgrsn 1:f102831401a8 196 //pid_x.reset();
sgrsn 1:f102831401a8 197 //pid_y.reset();
sgrsn 1:f102831401a8 198 //pid_theta.reset();
sgrsn 1:f102831401a8 199 wait_ms(500);
sgrsn 1:f102831401a8 200 jy901.reset();
sgrsn 0:f1459eec7228 201
sgrsn 0:f1459eec7228 202 // Gコードを次の行に
sgrsn 0:f1459eec7228 203 row++;
sgrsn 3:4ac32aff309c 204 if(row >= code.getGcodeSize())
sgrsn 1:f102831401a8 205 {
sgrsn 1:f102831401a8 206 row = 0;
sgrsn 3:4ac32aff309c 207 coastAllMotor();
sgrsn 3:4ac32aff309c 208 PC.printf("All task Clear\r\n");
sgrsn 3:4ac32aff309c 209 PC.printf("\r\nI'm ready to start. Press Key 'a'.\r\n");
sgrsn 3:4ac32aff309c 210 char input_character = 0;
sgrsn 3:4ac32aff309c 211 while(input_character != 'a')
sgrsn 3:4ac32aff309c 212 {
sgrsn 3:4ac32aff309c 213 if(PC.readable() > 0)
sgrsn 3:4ac32aff309c 214 {
sgrsn 3:4ac32aff309c 215 input_character = PC.getc();
sgrsn 3:4ac32aff309c 216 }
sgrsn 3:4ac32aff309c 217 }
sgrsn 1:f102831401a8 218 }
sgrsn 1:f102831401a8 219
sgrsn 3:4ac32aff309c 220 jy901.reset();
sgrsn 3:4ac32aff309c 221 v[0] = 0;
sgrsn 3:4ac32aff309c 222 v[1] = 0;
sgrsn 3:4ac32aff309c 223 v[2] = 0;
sgrsn 3:4ac32aff309c 224 v[3] = 0;
sgrsn 3:4ac32aff309c 225
sgrsn 1:f102831401a8 226 // 目標位置把握
sgrsn 0:f1459eec7228 227 code.getGcode(row, ArraySize(array), array);
sgrsn 1:f102831401a8 228 x_desire = array[0];
sgrsn 1:f102831401a8 229 y_desire = array[1];
sgrsn 1:f102831401a8 230 theta_desire = float(array[2]) *DEG_TO_RAD;
sgrsn 1:f102831401a8 231 pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG);
sgrsn 1:f102831401a8 232 path = 0;
sgrsn 0:f1459eec7228 233 }
sgrsn 0:f1459eec7228 234
sgrsn 0:f1459eec7228 235 // 目標速度計算
sgrsn 0:f1459eec7228 236 else
sgrsn 0:f1459eec7228 237 {
sgrsn 0:f1459eec7228 238 // 慣性座標での速度
sgrsn 1:f102831401a8 239 float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
sgrsn 1:f102831401a8 240 float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
sgrsn 1:f102831401a8 241 float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
sgrsn 1:f102831401a8 242 path++;
sgrsn 1:f102831401a8 243 if(path >= pathSize) path = pathSize-1;
sgrsn 0:f1459eec7228 244
sgrsn 0:f1459eec7228 245 // ロボット座標での速度
sgrsn 0:f1459eec7228 246 float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 247 float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 248
sgrsn 0:f1459eec7228 249 // 各車輪の速度
sgrsn 0:f1459eec7228 250 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 251 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 252 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 253 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 254
sgrsn 0:f1459eec7228 255 // 本当はこうするべき
sgrsn 0:f1459eec7228 256 // f = v * ppr / ( 2*PI * r);
sgrsn 0:f1459eec7228 257
sgrsn 0:f1459eec7228 258 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 259 {
sgrsn 1:f102831401a8 260 controlMotor(i, (int)v[i]);
sgrsn 0:f1459eec7228 261 }
sgrsn 0:f1459eec7228 262 }
sgrsn 0:f1459eec7228 263 }
sgrsn 0:f1459eec7228 264 }
sgrsn 0:f1459eec7228 265
sgrsn 0:f1459eec7228 266 int main()
sgrsn 0:f1459eec7228 267 {
sgrsn 0:f1459eec7228 268 coastAllMotor();
sgrsn 0:f1459eec7228 269 PC.baud(9600);
sgrsn 1:f102831401a8 270 //PC.attach(serialRead);
sgrsn 1:f102831401a8 271 jy901.calibrateAll(5000);
sgrsn 1:f102831401a8 272 //controlFromWASD();
sgrsn 3:4ac32aff309c 273 PC.printf("\r\nI'm ready to start. Press Key 'a'.\r\n");
sgrsn 2:83fa142c5bcc 274 //bool startable = false;
sgrsn 2:83fa142c5bcc 275 char input_character = 0;
sgrsn 2:83fa142c5bcc 276 while(input_character != 'a')
sgrsn 1:f102831401a8 277 {
sgrsn 2:83fa142c5bcc 278 if(PC.readable() > 0)
sgrsn 2:83fa142c5bcc 279 {
sgrsn 2:83fa142c5bcc 280 input_character = PC.getc();
sgrsn 2:83fa142c5bcc 281 }
sgrsn 1:f102831401a8 282 }
sgrsn 0:f1459eec7228 283 controlFromGcode();
sgrsn 0:f1459eec7228 284 }
sgrsn 0:f1459eec7228 285
sgrsn 1:f102831401a8 286 int controlMotor(int ch, int frequency)
sgrsn 0:f1459eec7228 287 {
sgrsn 0:f1459eec7228 288 int dir = COAST;
sgrsn 0:f1459eec7228 289 int size = 4;
sgrsn 0:f1459eec7228 290 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 291 {
sgrsn 0:f1459eec7228 292 //channel error
sgrsn 1:f102831401a8 293 return 0;
sgrsn 0:f1459eec7228 294 }
sgrsn 0:f1459eec7228 295 else
sgrsn 0:f1459eec7228 296 {
sgrsn 0:f1459eec7228 297 if(frequency > 0)
sgrsn 0:f1459eec7228 298 {
sgrsn 0:f1459eec7228 299 dir = CW;
sgrsn 0:f1459eec7228 300 }
sgrsn 0:f1459eec7228 301 else if(frequency < 0)
sgrsn 0:f1459eec7228 302 {
sgrsn 0:f1459eec7228 303 dir = CCW;
sgrsn 0:f1459eec7228 304 frequency = -frequency;
sgrsn 0:f1459eec7228 305 }
sgrsn 0:f1459eec7228 306 else
sgrsn 0:f1459eec7228 307 {
sgrsn 0:f1459eec7228 308 dir = BRAKE;
sgrsn 0:f1459eec7228 309 }
sgrsn 0:f1459eec7228 310 // 周波数制限 脱調を防ぐ
sgrsn 0:f1459eec7228 311 if(frequency > MaxFrequency) frequency = MaxFrequency;
sgrsn 1:f102831401a8 312 //else if(frequency < MinFrequency) frequency = MinFrequency;
sgrsn 0:f1459eec7228 313
sgrsn 0:f1459eec7228 314 master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
sgrsn 0:f1459eec7228 315 master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
sgrsn 1:f102831401a8 316
sgrsn 1:f102831401a8 317 return frequency;
sgrsn 0:f1459eec7228 318 }
sgrsn 0:f1459eec7228 319 }
sgrsn 0:f1459eec7228 320
sgrsn 0:f1459eec7228 321
sgrsn 0:f1459eec7228 322 void coastAllMotor()
sgrsn 0:f1459eec7228 323 {
sgrsn 0:f1459eec7228 324 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 325 {
sgrsn 0:f1459eec7228 326 master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
sgrsn 0:f1459eec7228 327 }
sgrsn 0:f1459eec7228 328 }
sgrsn 0:f1459eec7228 329
sgrsn 0:f1459eec7228 330 void serialRead()
sgrsn 0:f1459eec7228 331 {
sgrsn 0:f1459eec7228 332 int reg = 0;
sgrsn 0:f1459eec7228 333 uint8_t data[4] = {};
sgrsn 0:f1459eec7228 334 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 335 {
sgrsn 0:f1459eec7228 336 reg = PC.getc();
sgrsn 0:f1459eec7228 337 data[0] = PC.getc();
sgrsn 0:f1459eec7228 338 data[1] = PC.getc();
sgrsn 0:f1459eec7228 339 data[2] = PC.getc();
sgrsn 0:f1459eec7228 340 data[3] = PC.getc();
sgrsn 0:f1459eec7228 341 }
sgrsn 0:f1459eec7228 342 Register[reg % 0x20] = 0;
sgrsn 0:f1459eec7228 343 for(int i = 0; i < 4; i++)
sgrsn 0:f1459eec7228 344 Register[reg % 0x20] |= int(data[i]) << (i*8);
sgrsn 0:f1459eec7228 345 }
sgrsn 0:f1459eec7228 346
sgrsn 0:f1459eec7228 347
sgrsn 0:f1459eec7228 348
sgrsn 0:f1459eec7228 349 /*Function for Test***********************************************************/
sgrsn 0:f1459eec7228 350
sgrsn 0:f1459eec7228 351 void controlFrequencyFromTerminal()
sgrsn 0:f1459eec7228 352 {
sgrsn 0:f1459eec7228 353 int ch, speed;
sgrsn 0:f1459eec7228 354 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 355 {
sgrsn 0:f1459eec7228 356 PC.scanf("M%d:%d", &ch, &speed);
sgrsn 0:f1459eec7228 357 PC.printf("M%d:%d\r\n", ch, speed);
sgrsn 0:f1459eec7228 358 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 359 PC.printf("channnel error\r\n");
sgrsn 0:f1459eec7228 360 else
sgrsn 0:f1459eec7228 361 {
sgrsn 0:f1459eec7228 362 controlMotor(ch, speed);
sgrsn 0:f1459eec7228 363 }
sgrsn 0:f1459eec7228 364 }
sgrsn 0:f1459eec7228 365 }
sgrsn 0:f1459eec7228 366
sgrsn 0:f1459eec7228 367 void controlFromWASD()
sgrsn 0:f1459eec7228 368 {
sgrsn 0:f1459eec7228 369 float L = 4.0;
sgrsn 0:f1459eec7228 370 float v[4] = {};
sgrsn 0:f1459eec7228 371 char input = 0;
sgrsn 0:f1459eec7228 372 while(1)
sgrsn 0:f1459eec7228 373 {
sgrsn 0:f1459eec7228 374 if(PC.readable() > 0)
sgrsn 0:f1459eec7228 375 {
sgrsn 0:f1459eec7228 376 input = PC.getc();
sgrsn 0:f1459eec7228 377 //printf("%c\r\n", input);
sgrsn 0:f1459eec7228 378 }
sgrsn 0:f1459eec7228 379
sgrsn 0:f1459eec7228 380 float xI_dot = 0;
sgrsn 0:f1459eec7228 381 float yI_dot = 0;
sgrsn 0:f1459eec7228 382 switch(input)
sgrsn 0:f1459eec7228 383 {
sgrsn 0:f1459eec7228 384 case 'a':
sgrsn 0:f1459eec7228 385 xI_dot = -1;
sgrsn 0:f1459eec7228 386 yI_dot = 0;
sgrsn 0:f1459eec7228 387 break;
sgrsn 0:f1459eec7228 388 case 'd':
sgrsn 0:f1459eec7228 389 xI_dot = 1;
sgrsn 0:f1459eec7228 390 yI_dot = 0;
sgrsn 0:f1459eec7228 391 break;
sgrsn 0:f1459eec7228 392 case 'w':
sgrsn 0:f1459eec7228 393 xI_dot = 0;
sgrsn 0:f1459eec7228 394 yI_dot = 1;
sgrsn 0:f1459eec7228 395 break;
sgrsn 0:f1459eec7228 396 case 's':
sgrsn 0:f1459eec7228 397 xI_dot = 0;
sgrsn 0:f1459eec7228 398 yI_dot = -1;
sgrsn 0:f1459eec7228 399 break;
sgrsn 0:f1459eec7228 400 case ' ':
sgrsn 0:f1459eec7228 401 xI_dot = 0;
sgrsn 0:f1459eec7228 402 yI_dot = 0;
sgrsn 0:f1459eec7228 403 break;
sgrsn 0:f1459eec7228 404 }
sgrsn 0:f1459eec7228 405 //master.getSlaveRegistarData(addr, 1, &data, size);
sgrsn 0:f1459eec7228 406
sgrsn 0:f1459eec7228 407 float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 0:f1459eec7228 408
sgrsn 0:f1459eec7228 409 float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 410 float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot;
sgrsn 0:f1459eec7228 411 float theta_dot = 0.0 - theta_z;
sgrsn 0:f1459eec7228 412
sgrsn 0:f1459eec7228 413 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 414 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 415 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 416 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 417
sgrsn 0:f1459eec7228 418 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 419 {
sgrsn 0:f1459eec7228 420 controlMotor(i, v[i] * 20000.0);
sgrsn 0:f1459eec7228 421 }
sgrsn 0:f1459eec7228 422
sgrsn 0:f1459eec7228 423 PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]);
sgrsn 0:f1459eec7228 424
sgrsn 0:f1459eec7228 425 //PC.printf("%f\r\n", theta_z);
sgrsn 0:f1459eec7228 426 }
sgrsn 0:f1459eec7228 427 }