WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
Diff: main.cpp
- Revision:
- 1:f102831401a8
- Parent:
- 0:f1459eec7228
- Child:
- 2:83fa142c5bcc
--- a/main.cpp Mon Dec 16 10:38:07 2019 +0000 +++ b/main.cpp Tue Dec 17 04:41:19 2019 +0000 @@ -13,12 +13,91 @@ #define SIZE 6 #define ArraySize(array) (sizeof(array) / sizeof(array[0])) -float DEG_TO_RAD = PI/180.0; +float wheel_d = 127; // メカナム直径[mm] +float wheel_r = 63.5; +float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) -void controlMotor(int ch, int frequency); +float DEG_TO_RAD = PI/180.0; +float RAD_TO_DEG = 180.0/PI; + +int controlMotor(int ch, int frequency); void coastAllMotor(); void controlFrequencyFromTerminal(); void serialRead(); +void controlFromWASD(); + +class CountWheel +{ + public: + CountWheel(Timer *t) + { + _t = t; + _t -> start(); + } + float getRadian(float frequency) + { + last_time = current_time; + current_time = _t -> read(); + float dt = current_time - last_time; + float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD; + return delta_rad; + } + + private: + Timer *_t; + float last_time; + float current_time; +}; + +class MakePath +{ + public: + MakePath() + { + } + int makePath(int targetX, int targetY, int targetZ, int x, int y, int z) + { + int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0; //5mm間隔 + //printf("num = %d\r\n", num); + for(int i = 1; i <= num; i++) + { + float a = float(i) / num; + PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); + PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); + PATH[i-1][2] = z + float(targetZ - z) * a; + } + if(num > 0) + { + PATH[num-1][0] = targetX; + PATH[num-1][1] = targetY; + PATH[num-1][2] = targetZ; + } + else + { + PATH[0][0] = targetX; + PATH[0][1] = targetY; + PATH[0][2] = targetZ; + num = 1; + } + return num; + } + + int getPathX(int i) + { + return PATH[i][0]; + } + int getPathY(int i) + { + return PATH[i][1]; + } + int getPathZ(int i) + { + return PATH[i][2]; + } + + private: + int PATH[500][3]; +}; int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; int Register[0x20] = {}; @@ -28,12 +107,12 @@ BusOut LEDs(LED1, LED2, LED3, LED4); Timer timer; JY901 jy901(&master, &timer); - +MakePath myPath; void controlFromGcode() { - float threshold_x = 0; //[mm] - float threshold_y = 0; //[mm] + float threshold_x = 5; //[mm] + float threshold_y = 5; //[mm] float threshold_theta = 5 * DEG_TO_RAD; // 角度補正係数 @@ -43,9 +122,9 @@ PID pid_x(&timer2); PID pid_y(&timer2); PID pid_theta(&timer2); - pid_x.setParameter(100.0, 0.0, 0.0); - pid_y.setParameter(100.0, 0.0, 0.0); - pid_theta.setParameter(1.0, 0.0, 0.0); + pid_x.setParameter(200.0, 0.0, 0.0); + pid_y.setParameter(200.0, 0.0, 0.0); + pid_theta.setParameter(100.0, 0.0, 0.0); // Gコード読み取り LocalFileSystem local("local"); @@ -56,48 +135,89 @@ int row = 1; float v[4] = {}; - TextLCD lcd(p24, p26, p27, p28, p29, p30); + Timer temp_t; + CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)}; + float wheel_rad[4] = {}; + + //TextLCD lcd(p24, p26, p27, p28, p29, p30); + + float x_robot = 0; + float y_robot = 0; + float theta_robot = 0; + + + // 目標位置把握 + code.getGcode(row,sizeof(array)/sizeof(int),array); + float x_desire = array[0]; + float y_desire = array[1]; + float theta_desire = float(array[2]) *DEG_TO_RAD; + int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot); + + int path = 0; while(1) { // 自己位置推定 - float x_robot = Register[MARKER_X]; - float y_robot = Register[MARKER_Y]; - float theta_robot = float(Register[MARKER_YAW]) / 1000.0; - float theta_robot_formJyro = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; + //float x_robot = Register[MARKER_X]; + //float y_robot = Register[MARKER_Y]; + //float theta_robot = float(Register[MARKER_YAW]) / 1000.0; + theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; + for(int i = 0; i < MOTOR_NUM; i++) + { + wheel_rad[i] = counter[i].getRadian(v[i]); + } + float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.7; + float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.7; - lcd.printf("%d,%d,%d\n", (int)x_robot, (int)y_robot, (int)(theta_robot*180/PI)); - - // 目標位置把握 - code.getGcode(row,sizeof(array)/sizeof(int),array); - float x_desire = array[0]; - float y_desire = array[1]; - float theta_desire = float(array[2]) *DEG_TO_RAD; + x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot); + y_robot += dy * cos(theta_robot) + dx * sin(theta_robot); // 目標位置判定 float err_x = x_desire - x_robot; float err_y = y_desire - y_robot; float err_theta = theta_desire - theta_robot; + //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row); + printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row); + //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row); + // 目標位置到達 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) { // 車輪を停止 coastAllMotor(); + //pid_x.reset(); + //pid_y.reset(); + //pid_theta.reset(); + wait_ms(500); + jy901.reset(); // Gコードを次の行に row++; + if(row > code.getGcodeSize()) + { + row = 0; + } + + // 目標位置把握 code.getGcode(row, ArraySize(array), array); + x_desire = array[0]; + y_desire = array[1]; + theta_desire = float(array[2]) *DEG_TO_RAD; + pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG); + path = 0; } // 目標速度計算 else { // 慣性座標での速度 - float xI_dot = pid_x.controlPID(x_desire, x_robot); - float yI_dot = pid_y.controlPID(y_desire, y_robot); - float theta_dot = pid_theta.controlPID(theta_desire, theta_robot); + float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); + float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); + float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); + path++; + if(path >= pathSize) path = pathSize-1; // ロボット座標での速度 float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; @@ -114,7 +234,7 @@ for(int i = 0; i < MOTOR_NUM; i++) { - controlMotor(i, (int)v[i] ); + controlMotor(i, (int)v[i]); } } } @@ -124,20 +244,26 @@ { coastAllMotor(); PC.baud(9600); - PC.attach(serialRead); - //jy901.calibrateAll(5000); - + //PC.attach(serialRead); + jy901.calibrateAll(5000); + //controlFromWASD(); + PC.printf("\r\nI'm ready to start. Press Enter\r\n"); + bool startable = false; + while(!startable) + { + startable = PC.readable() > 0; + } controlFromGcode(); } - -void controlMotor(int ch, int frequency) +int controlMotor(int ch, int frequency) { int dir = COAST; int size = 4; if(ch < 0 || ch > 3) { //channel error + return 0; } else { @@ -156,9 +282,12 @@ } // 周波数制限 脱調を防ぐ if(frequency > MaxFrequency) frequency = MaxFrequency; + //else if(frequency < MinFrequency) frequency = MinFrequency; master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); + + return frequency; } }