WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
main.cpp
- Committer:
- sgrsn
- Date:
- 2019-12-16
- Revision:
- 0:f1459eec7228
- Child:
- 1:f102831401a8
File content as of revision 0:f1459eec7228:
#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 DEG_TO_RAD = PI/180.0; void controlMotor(int ch, int frequency); void coastAllMotor(); void controlFrequencyFromTerminal(); void serialRead(); 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); void controlFromGcode() { float threshold_x = 0; //[mm] float threshold_y = 0; //[mm] float threshold_theta = 5 * DEG_TO_RAD; // 角度補正係数 float L = 233; //[mm] Timer timer2; 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); // Gコード読み取り LocalFileSystem local("local"); int array[SIZE] = {}; FILE *fp = fopen( "/local/guide1.txt", "r"); MakeSequencer code(fp); int row = 1; float v[4] = {}; TextLCD lcd(p24, p26, p27, p28, p29, p30); 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; 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; // 目標位置判定 float err_x = x_desire - x_robot; float err_y = y_desire - y_robot; float err_theta = theta_desire - theta_robot; // 目標位置到達 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) { // 車輪を停止 coastAllMotor(); // Gコードを次の行に row++; code.getGcode(row, ArraySize(array), array); } // 目標速度計算 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 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); controlFromGcode(); } void controlMotor(int ch, int frequency) { int dir = COAST; int size = 4; if(ch < 0 || ch > 3) { //channel error } else { if(frequency > 0) { dir = CW; } else if(frequency < 0) { dir = CCW; frequency = -frequency; } else { dir = BRAKE; } // 周波数制限 脱調を防ぐ if(frequency > MaxFrequency) frequency = MaxFrequency; master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); } } 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); } }