hidaka sato / Mbed 2 deprecated WRS2019_master

WRS2019

Dependencies:   mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3

main.cpp@4:25ab7216447f, 2019-12-18 (annotated)

Committer:
sgrsn
Date:
Wed Dec 18 02:24:18 2019 +0000
Branch:
StartFromROS
Revision:
4:25ab7216447f
Parent:
3:4ac32aff309c
Child:
5:a5dc3090ba44
Change fro ROS

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