WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
main.cpp
- Committer:
- sgrsn
- Date:
- 2019-12-17
- Revision:
- 2:83fa142c5bcc
- Parent:
- 1:f102831401a8
- Child:
- 3:4ac32aff309c
File content as of revision 2:83fa142c5bcc:
#include "mbed.h" #include <string> #include "i2cmaster.h" #include "JY901.h" #include "PID.h" #include "MakeSequencer.h" #include "define.h" #include "TextLCD.h" // MakeSequencer #define SIZE 6 #define ArraySize(array) (sizeof(array) / sizeof(array[0])) float wheel_d = 127; // メカナム直径[mm] float wheel_r = 63.5; float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) 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] = {}; Serial PC(USBTX, USBRX); i2c master(p28, p27); BusOut LEDs(LED1, LED2, LED3, LED4); Timer timer; JY901 jy901(&master, &timer); MakePath myPath; void controlFromGcode() { float threshold_x = 5; //[mm] float threshold_y = 5; //[mm] float threshold_theta = 1 * DEG_TO_RAD; // 角度補正係数 float L = 233; //[mm] Timer timer2; PID pid_x(&timer2); PID pid_y(&timer2); PID pid_theta(&timer2); 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"); int array[SIZE] = {}; FILE *fp = fopen( "/local/guide1.txt", "r"); MakeSequencer code(fp); int row = 1; float v[4] = {}; 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; 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.67; float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67; 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(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; float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; // 各車輪の速度 v[0] = -x_dot - y_dot - L * theta_dot; v[1] = x_dot - y_dot - L * theta_dot; v[2] = x_dot + y_dot - L * theta_dot; v[3] = -x_dot + y_dot - L * theta_dot; // 本当はこうするべき // f = v * ppr / ( 2*PI * r); for(int i = 0; i < MOTOR_NUM; i++) { controlMotor(i, (int)v[i]); } } } } int main() { coastAllMotor(); PC.baud(9600); //PC.attach(serialRead); jy901.calibrateAll(5000); //controlFromWASD(); PC.printf("\r\nI'm ready to start. Press Enter\r\n"); //bool startable = false; char input_character = 0; while(input_character != 'a') { if(PC.readable() > 0) { input_character = PC.getc(); } } controlFromGcode(); } int controlMotor(int ch, int frequency) { int dir = COAST; int size = 4; if(ch < 0 || ch > 3) { //channel error return 0; } else { if(frequency > 0) { dir = CW; } else if(frequency < 0) { dir = CCW; frequency = -frequency; } else { dir = BRAKE; } // 周波数制限 脱調を防ぐ 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; } } void coastAllMotor() { for(int i = 0; i < MOTOR_NUM; i++) { master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4); } } void serialRead() { int reg = 0; uint8_t data[4] = {}; if(PC.readable() > 0) { reg = PC.getc(); data[0] = PC.getc(); data[1] = PC.getc(); data[2] = PC.getc(); data[3] = PC.getc(); } Register[reg % 0x20] = 0; for(int i = 0; i < 4; i++) Register[reg % 0x20] |= int(data[i]) << (i*8); } /*Function for Test***********************************************************/ void controlFrequencyFromTerminal() { int ch, speed; if(PC.readable() > 0) { PC.scanf("M%d:%d", &ch, &speed); PC.printf("M%d:%d\r\n", ch, speed); if(ch < 0 || ch > 3) PC.printf("channnel error\r\n"); else { controlMotor(ch, speed); } } } void controlFromWASD() { float L = 4.0; float v[4] = {}; char input = 0; while(1) { if(PC.readable() > 0) { input = PC.getc(); //printf("%c\r\n", input); } float xI_dot = 0; float yI_dot = 0; switch(input) { case 'a': xI_dot = -1; yI_dot = 0; break; case 'd': xI_dot = 1; yI_dot = 0; break; case 'w': xI_dot = 0; yI_dot = 1; break; case 's': xI_dot = 0; yI_dot = -1; break; case ' ': xI_dot = 0; yI_dot = 0; break; } //master.getSlaveRegistarData(addr, 1, &data, size); float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot; float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot; float theta_dot = 0.0 - theta_z; v[1] = x_dot - y_dot - L * theta_dot; v[2] = x_dot + y_dot - L * theta_dot; v[3] = -x_dot + y_dot - L * theta_dot; v[0] = -x_dot - y_dot - L * theta_dot; for(int i = 0; i < MOTOR_NUM; i++) { controlMotor(i, v[i] * 20000.0); } PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]); //PC.printf("%f\r\n", theta_z); } }