hidaka sato / Mbed 2 deprecated WRS2019_master

WRS2019

Dependencies:   mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3

main.cpp@6:55e60b9d7ff1, 2019-12-18 (annotated)

Committer:
sgrsn
Date:
Wed Dec 18 02:59:04 2019 +0000
Branch:
StartFromROS
Revision:
6:55e60b9d7ff1
Parent:
5:a5dc3090ba44 
Fix that BuffredSerial be in ros_lib_kinetic.

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 4:25ab7216447f 9 #include <ros.h>
sgrsn 4:25ab7216447f 10 #include <std_msgs/Empty.h>
sgrsn 4:25ab7216447f 11
sgrsn 0:f1459eec7228 12 #include "TextLCD.h"
sgrsn 0:f1459eec7228 13
sgrsn 4:25ab7216447f 14 // robot start when startable is true
sgrsn 4:25ab7216447f 15 bool startable = false;
sgrsn 4:25ab7216447f 16
sgrsn 4:25ab7216447f 17 // for ROS
sgrsn 4:25ab7216447f 18 ros::NodeHandle nh;
sgrsn 4:25ab7216447f 19 void messageCb(const std_msgs::Empty& toggle_msg){
sgrsn 4:25ab7216447f 20 startable = true;
sgrsn 4:25ab7216447f 21 }
sgrsn 4:25ab7216447f 22 ros::Subscriber<std_msgs::Empty> sub("/robot/start", &messageCb);
sgrsn 0:f1459eec7228 23
sgrsn 0:f1459eec7228 24 // MakeSequencer
sgrsn 0:f1459eec7228 25 #define SIZE 6
sgrsn 0:f1459eec7228 26 #define ArraySize(array) (sizeof(array) / sizeof(array[0]))
sgrsn 0:f1459eec7228 27
sgrsn 5:a5dc3090ba44 28 // Robot Parameter
sgrsn 1:f102831401a8 29 float wheel_d = 127; // メカナム直径[mm]
sgrsn 1:f102831401a8 30 float wheel_r = 63.5;
sgrsn 1:f102831401a8 31 float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1)
sgrsn 0:f1459eec7228 32
sgrsn 1:f102831401a8 33 float DEG_TO_RAD = PI/180.0;
sgrsn 1:f102831401a8 34 float RAD_TO_DEG = 180.0/PI;
sgrsn 1:f102831401a8 35
sgrsn 5:a5dc3090ba44 36 // Prototype
sgrsn 1:f102831401a8 37 int controlMotor(int ch, int frequency);
sgrsn 0:f1459eec7228 38 void coastAllMotor();
sgrsn 1:f102831401a8 39
sgrsn 5:a5dc3090ba44 40 // this class Count Stepping Motor Pulse for wheel
sgrsn 1:f102831401a8 41 class CountWheel
sgrsn 1:f102831401a8 42 {
sgrsn 1:f102831401a8 43 public:
sgrsn 1:f102831401a8 44 CountWheel(Timer *t)
sgrsn 1:f102831401a8 45 {
sgrsn 1:f102831401a8 46 _t = t;
sgrsn 1:f102831401a8 47 _t -> start();
sgrsn 1:f102831401a8 48 }
sgrsn 1:f102831401a8 49 float getRadian(float frequency)
sgrsn 1:f102831401a8 50 {
sgrsn 1:f102831401a8 51 last_time = current_time;
sgrsn 1:f102831401a8 52 current_time = _t -> read();
sgrsn 1:f102831401a8 53 float dt = current_time - last_time;
sgrsn 1:f102831401a8 54 float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
sgrsn 1:f102831401a8 55 return delta_rad;
sgrsn 1:f102831401a8 56 }
sgrsn 1:f102831401a8 57
sgrsn 1:f102831401a8 58 private:
sgrsn 1:f102831401a8 59 Timer *_t;
sgrsn 1:f102831401a8 60 float last_time;
sgrsn 1:f102831401a8 61 float current_time;
sgrsn 1:f102831401a8 62 };
sgrsn 1:f102831401a8 63
sgrsn 5:a5dc3090ba44 64 // this class make Robot Path to complement target value linearly
sgrsn 1:f102831401a8 65 class MakePath
sgrsn 1:f102831401a8 66 {
sgrsn 1:f102831401a8 67 public:
sgrsn 1:f102831401a8 68 MakePath()
sgrsn 1:f102831401a8 69 {
sgrsn 1:f102831401a8 70 }
sgrsn 1:f102831401a8 71 int makePath(int targetX, int targetY, int targetZ, int x, int y, int z)
sgrsn 1:f102831401a8 72 {
sgrsn 1:f102831401a8 73 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 74 //printf("num = %d\r\n", num);
sgrsn 1:f102831401a8 75 for(int i = 1; i <= num; i++)
sgrsn 1:f102831401a8 76 {
sgrsn 1:f102831401a8 77 float a = float(i) / num;
sgrsn 1:f102831401a8 78 PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD);
sgrsn 1:f102831401a8 79 PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD);
sgrsn 1:f102831401a8 80 PATH[i-1][2] = z + float(targetZ - z) * a;
sgrsn 1:f102831401a8 81 }
sgrsn 1:f102831401a8 82 if(num > 0)
sgrsn 1:f102831401a8 83 {
sgrsn 1:f102831401a8 84 PATH[num-1][0] = targetX;
sgrsn 1:f102831401a8 85 PATH[num-1][1] = targetY;
sgrsn 1:f102831401a8 86 PATH[num-1][2] = targetZ;
sgrsn 1:f102831401a8 87 }
sgrsn 1:f102831401a8 88 else
sgrsn 1:f102831401a8 89 {
sgrsn 1:f102831401a8 90 PATH[0][0] = targetX;
sgrsn 1:f102831401a8 91 PATH[0][1] = targetY;
sgrsn 1:f102831401a8 92 PATH[0][2] = targetZ;
sgrsn 1:f102831401a8 93 num = 1;
sgrsn 1:f102831401a8 94 }
sgrsn 1:f102831401a8 95 return num;
sgrsn 1:f102831401a8 96 }
sgrsn 1:f102831401a8 97
sgrsn 1:f102831401a8 98 int getPathX(int i)
sgrsn 1:f102831401a8 99 {
sgrsn 1:f102831401a8 100 return PATH[i][0];
sgrsn 1:f102831401a8 101 }
sgrsn 1:f102831401a8 102 int getPathY(int i)
sgrsn 1:f102831401a8 103 {
sgrsn 1:f102831401a8 104 return PATH[i][1];
sgrsn 1:f102831401a8 105 }
sgrsn 1:f102831401a8 106 int getPathZ(int i)
sgrsn 1:f102831401a8 107 {
sgrsn 1:f102831401a8 108 return PATH[i][2];
sgrsn 1:f102831401a8 109 }
sgrsn 1:f102831401a8 110
sgrsn 1:f102831401a8 111 private:
sgrsn 1:f102831401a8 112 int PATH[500][3];
sgrsn 1:f102831401a8 113 };
sgrsn 0:f1459eec7228 114
sgrsn 5:a5dc3090ba44 115 // IIC address
sgrsn 0:f1459eec7228 116 int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
sgrsn 0:f1459eec7228 117 int Register[0x20] = {};
sgrsn 0:f1459eec7228 118
sgrsn 0:f1459eec7228 119 i2c master(p28, p27);
sgrsn 0:f1459eec7228 120 BusOut LEDs(LED1, LED2, LED3, LED4);
sgrsn 0:f1459eec7228 121 Timer timer;
sgrsn 5:a5dc3090ba44 122 JY901 jy901(&master, &timer); // gyro sensor
sgrsn 1:f102831401a8 123 MakePath myPath;
sgrsn 0:f1459eec7228 124
sgrsn 5:a5dc3090ba44 125 // this function is Robot Main Program
sgrsn 0:f1459eec7228 126 void controlFromGcode()
sgrsn 0:f1459eec7228 127 {
sgrsn 5:a5dc3090ba44 128 // Threshold for reaching target value
sgrsn 1:f102831401a8 129 float threshold_x = 5; //[mm]
sgrsn 1:f102831401a8 130 float threshold_y = 5; //[mm]
sgrsn 2:83fa142c5bcc 131 float threshold_theta = 1 * DEG_TO_RAD;
sgrsn 0:f1459eec7228 132
sgrsn 0:f1459eec7228 133 // 角度補正係数
sgrsn 0:f1459eec7228 134 float L = 233; //[mm]
sgrsn 0:f1459eec7228 135
sgrsn 0:f1459eec7228 136 Timer timer2;
sgrsn 0:f1459eec7228 137 PID pid_x(&timer2);
sgrsn 0:f1459eec7228 138 PID pid_y(&timer2);
sgrsn 0:f1459eec7228 139 PID pid_theta(&timer2);
sgrsn 1:f102831401a8 140 pid_x.setParameter(200.0, 0.0, 0.0);
sgrsn 1:f102831401a8 141 pid_y.setParameter(200.0, 0.0, 0.0);
sgrsn 1:f102831401a8 142 pid_theta.setParameter(100.0, 0.0, 0.0);
sgrsn 0:f1459eec7228 143
sgrsn 5:a5dc3090ba44 144 // read the txt file for get target position
sgrsn 0:f1459eec7228 145 // Gコード読み取り
sgrsn 0:f1459eec7228 146 LocalFileSystem local("local");
sgrsn 0:f1459eec7228 147 int array[SIZE] = {};
sgrsn 0:f1459eec7228 148 FILE *fp = fopen( "/local/guide1.txt", "r");
sgrsn 0:f1459eec7228 149 MakeSequencer code(fp);
sgrsn 4:25ab7216447f 150
sgrsn 4:25ab7216447f 151 //printf("size:%d\r\n", code.getGcodeSize());
sgrsn 3:4ac32aff309c 152 for(int i = 0; i < code.getGcodeSize(); i++)
sgrsn 3:4ac32aff309c 153 {
sgrsn 3:4ac32aff309c 154 code.getGcode(i,sizeof(array)/sizeof(int),array);
sgrsn 3:4ac32aff309c 155 printf("%d, %d, %d\r\n", array[0], array[1], i);
sgrsn 3:4ac32aff309c 156 }
sgrsn 0:f1459eec7228 157
sgrsn 5:a5dc3090ba44 158 int row = 1; // row of txt file
sgrsn 5:a5dc3090ba44 159 float v[4] = {}; // frequency each wheel
sgrsn 0:f1459eec7228 160
sgrsn 1:f102831401a8 161 Timer temp_t;
sgrsn 1:f102831401a8 162 CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)};
sgrsn 1:f102831401a8 163 float wheel_rad[4] = {};
sgrsn 1:f102831401a8 164
sgrsn 5:a5dc3090ba44 165 //TextLCD lcd(p24, p26, p27, p28, p29, p30);
sgrsn 1:f102831401a8 166
sgrsn 1:f102831401a8 167 float x_robot = 0;
sgrsn 1:f102831401a8 168 float y_robot = 0;
sgrsn 1:f102831401a8 169 float theta_robot = 0;
sgrsn 1:f102831401a8 170
sgrsn 1:f102831401a8 171
sgrsn 1:f102831401a8 172 // 目標位置把握
sgrsn 1:f102831401a8 173 code.getGcode(row,sizeof(array)/sizeof(int),array);
sgrsn 1:f102831401a8 174 float x_desire = array[0];
sgrsn 1:f102831401a8 175 float y_desire = array[1];
sgrsn 1:f102831401a8 176 float theta_desire = float(array[2]) *DEG_TO_RAD;
sgrsn 1:f102831401a8 177 int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot);
sgrsn 1:f102831401a8 178
sgrsn 1:f102831401a8 179 int path = 0;
sgrsn 0:f1459eec7228 180
sgrsn 0:f1459eec7228 181 while(1)
sgrsn 0:f1459eec7228 182 {
sgrsn 4:25ab7216447f 183 nh.spinOnce();
sgrsn 5:a5dc3090ba44 184 // 自己位置推定(Estimate Self-localization)
sgrsn 1:f102831401a8 185 theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 1:f102831401a8 186 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 1:f102831401a8 187 {
sgrsn 1:f102831401a8 188 wheel_rad[i] = counter[i].getRadian(v[i]);
sgrsn 1:f102831401a8 189 }
sgrsn 2:83fa142c5bcc 190 float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
sgrsn 2:83fa142c5bcc 191 float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
sgrsn 0:f1459eec7228 192
sgrsn 1:f102831401a8 193 x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot);
sgrsn 1:f102831401a8 194 y_robot += dy * cos(theta_robot) + dx * sin(theta_robot);
sgrsn 0:f1459eec7228 195
sgrsn 5:a5dc3090ba44 196 // caliculate error of current position
sgrsn 0:f1459eec7228 197 // 目標位置判定
sgrsn 0:f1459eec7228 198 float err_x = x_desire - x_robot;
sgrsn 0:f1459eec7228 199 float err_y = y_desire - y_robot;
sgrsn 0:f1459eec7228 200 float err_theta = theta_desire - theta_robot;
sgrsn 0:f1459eec7228 201
sgrsn 1:f102831401a8 202 //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row);
sgrsn 5:a5dc3090ba44 203 //lcd.printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row);
sgrsn 1:f102831401a8 204 //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row);
sgrsn 1:f102831401a8 205
sgrsn 5:a5dc3090ba44 206 // if Reach target position
sgrsn 0:f1459eec7228 207 // 目標位置到達
sgrsn 0:f1459eec7228 208 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
sgrsn 0:f1459eec7228 209 {
sgrsn 5:a5dc3090ba44 210 // At first, Stop all Motor
sgrsn 0:f1459eec7228 211 // 車輪を停止
sgrsn 0:f1459eec7228 212 coastAllMotor();
sgrsn 1:f102831401a8 213 //pid_x.reset();
sgrsn 1:f102831401a8 214 //pid_y.reset();
sgrsn 1:f102831401a8 215 //pid_theta.reset();
sgrsn 1:f102831401a8 216 wait_ms(500);
sgrsn 5:a5dc3090ba44 217
sgrsn 5:a5dc3090ba44 218 // gyro reset
sgrsn 1:f102831401a8 219 jy901.reset();
sgrsn 0:f1459eec7228 220
sgrsn 5:a5dc3090ba44 221 // update target position
sgrsn 0:f1459eec7228 222 // Gコードを次の行に
sgrsn 0:f1459eec7228 223 row++;
sgrsn 3:4ac32aff309c 224 if(row >= code.getGcodeSize())
sgrsn 1:f102831401a8 225 {
sgrsn 1:f102831401a8 226 row = 0;
sgrsn 3:4ac32aff309c 227 coastAllMotor();
sgrsn 4:25ab7216447f 228 startable = false;
sgrsn 4:25ab7216447f 229 while(!startable)
sgrsn 4:25ab7216447f 230 {
sgrsn 4:25ab7216447f 231 nh.spinOnce();
sgrsn 4:25ab7216447f 232 wait_ms(1);
sgrsn 4:25ab7216447f 233 }
sgrsn 1:f102831401a8 234 }
sgrsn 1:f102831401a8 235
sgrsn 3:4ac32aff309c 236 jy901.reset();
sgrsn 3:4ac32aff309c 237 v[0] = 0;
sgrsn 3:4ac32aff309c 238 v[1] = 0;
sgrsn 3:4ac32aff309c 239 v[2] = 0;
sgrsn 3:4ac32aff309c 240 v[3] = 0;
sgrsn 3:4ac32aff309c 241
sgrsn 5:a5dc3090ba44 242 // get next target position
sgrsn 1:f102831401a8 243 // 目標位置把握
sgrsn 0:f1459eec7228 244 code.getGcode(row, ArraySize(array), array);
sgrsn 1:f102831401a8 245 x_desire = array[0];
sgrsn 1:f102831401a8 246 y_desire = array[1];
sgrsn 1:f102831401a8 247 theta_desire = float(array[2]) *DEG_TO_RAD;
sgrsn 1:f102831401a8 248 pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG);
sgrsn 1:f102831401a8 249 path = 0;
sgrsn 0:f1459eec7228 250 }
sgrsn 0:f1459eec7228 251
sgrsn 5:a5dc3090ba44 252 // caliculate velosity
sgrsn 0:f1459eec7228 253 // 目標速度計算
sgrsn 0:f1459eec7228 254 else
sgrsn 0:f1459eec7228 255 {
sgrsn 0:f1459eec7228 256 // 慣性座標での速度
sgrsn 1:f102831401a8 257 float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
sgrsn 1:f102831401a8 258 float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
sgrsn 1:f102831401a8 259 float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
sgrsn 1:f102831401a8 260 path++;
sgrsn 1:f102831401a8 261 if(path >= pathSize) path = pathSize-1;
sgrsn 0:f1459eec7228 262
sgrsn 0:f1459eec7228 263 // ロボット座標での速度
sgrsn 0:f1459eec7228 264 float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 265 float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
sgrsn 0:f1459eec7228 266
sgrsn 0:f1459eec7228 267 // 各車輪の速度
sgrsn 0:f1459eec7228 268 v[0] = -x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 269 v[1] = x_dot - y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 270 v[2] = x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 271 v[3] = -x_dot + y_dot - L * theta_dot;
sgrsn 0:f1459eec7228 272
sgrsn 0:f1459eec7228 273 // 本当はこうするべき
sgrsn 0:f1459eec7228 274 // f = v * ppr / ( 2*PI * r);
sgrsn 0:f1459eec7228 275
sgrsn 5:a5dc3090ba44 276 // Control Motor velocity
sgrsn 0:f1459eec7228 277 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 278 {
sgrsn 1:f102831401a8 279 controlMotor(i, (int)v[i]);
sgrsn 0:f1459eec7228 280 }
sgrsn 0:f1459eec7228 281 }
sgrsn 0:f1459eec7228 282 }
sgrsn 0:f1459eec7228 283 }
sgrsn 0:f1459eec7228 284
sgrsn 0:f1459eec7228 285 int main()
sgrsn 0:f1459eec7228 286 {
sgrsn 4:25ab7216447f 287 nh.getHardware()->setBaud(9600);
sgrsn 4:25ab7216447f 288 nh.initNode();
sgrsn 4:25ab7216447f 289 nh.subscribe(sub);
sgrsn 0:f1459eec7228 290 coastAllMotor();
sgrsn 5:a5dc3090ba44 291 // gyro sensor calibration
sgrsn 5:a5dc3090ba44 292 // Please wait 5 seconds...
sgrsn 1:f102831401a8 293 jy901.calibrateAll(5000);
sgrsn 5:a5dc3090ba44 294
sgrsn 5:a5dc3090ba44 295 // Robot start when receive the ROS topic
sgrsn 4:25ab7216447f 296 while(!startable)
sgrsn 1:f102831401a8 297 {
sgrsn 4:25ab7216447f 298 nh.spinOnce();
sgrsn 4:25ab7216447f 299 wait_ms(1);
sgrsn 1:f102831401a8 300 }
sgrsn 4:25ab7216447f 301 // main function
sgrsn 0:f1459eec7228 302 controlFromGcode();
sgrsn 0:f1459eec7228 303 }
sgrsn 0:f1459eec7228 304
sgrsn 1:f102831401a8 305 int controlMotor(int ch, int frequency)
sgrsn 0:f1459eec7228 306 {
sgrsn 0:f1459eec7228 307 int dir = COAST;
sgrsn 0:f1459eec7228 308 int size = 4;
sgrsn 0:f1459eec7228 309 if(ch < 0 || ch > 3)
sgrsn 0:f1459eec7228 310 {
sgrsn 0:f1459eec7228 311 //channel error
sgrsn 1:f102831401a8 312 return 0;
sgrsn 0:f1459eec7228 313 }
sgrsn 0:f1459eec7228 314 else
sgrsn 0:f1459eec7228 315 {
sgrsn 0:f1459eec7228 316 if(frequency > 0)
sgrsn 0:f1459eec7228 317 {
sgrsn 0:f1459eec7228 318 dir = CW;
sgrsn 0:f1459eec7228 319 }
sgrsn 0:f1459eec7228 320 else if(frequency < 0)
sgrsn 0:f1459eec7228 321 {
sgrsn 0:f1459eec7228 322 dir = CCW;
sgrsn 0:f1459eec7228 323 frequency = -frequency;
sgrsn 0:f1459eec7228 324 }
sgrsn 0:f1459eec7228 325 else
sgrsn 0:f1459eec7228 326 {
sgrsn 0:f1459eec7228 327 dir = BRAKE;
sgrsn 0:f1459eec7228 328 }
sgrsn 0:f1459eec7228 329 // 周波数制限 脱調を防ぐ
sgrsn 0:f1459eec7228 330 if(frequency > MaxFrequency) frequency = MaxFrequency;
sgrsn 1:f102831401a8 331 //else if(frequency < MinFrequency) frequency = MinFrequency;
sgrsn 0:f1459eec7228 332
sgrsn 0:f1459eec7228 333 master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
sgrsn 0:f1459eec7228 334 master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
sgrsn 1:f102831401a8 335
sgrsn 1:f102831401a8 336 return frequency;
sgrsn 0:f1459eec7228 337 }
sgrsn 0:f1459eec7228 338 }
sgrsn 0:f1459eec7228 339
sgrsn 0:f1459eec7228 340
sgrsn 0:f1459eec7228 341 void coastAllMotor()
sgrsn 0:f1459eec7228 342 {
sgrsn 0:f1459eec7228 343 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:f1459eec7228 344 {
sgrsn 0:f1459eec7228 345 master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
sgrsn 0:f1459eec7228 346 }
sgrsn 4:25ab7216447f 347 }