WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
Diff: main.cpp
- Branch:
- StartFromROS
- Revision:
- 5:a5dc3090ba44
- Parent:
- 4:25ab7216447f
--- a/main.cpp Wed Dec 18 02:24:18 2019 +0000 +++ b/main.cpp Wed Dec 18 02:51:29 2019 +0000 @@ -25,6 +25,7 @@ #define SIZE 6 #define ArraySize(array) (sizeof(array) / sizeof(array[0])) +// Robot Parameter float wheel_d = 127; // メカナム直径[mm] float wheel_r = 63.5; float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) @@ -32,9 +33,11 @@ float DEG_TO_RAD = PI/180.0; float RAD_TO_DEG = 180.0/PI; +// Prototype int controlMotor(int ch, int frequency); void coastAllMotor(); +// this class Count Stepping Motor Pulse for wheel class CountWheel { public: @@ -58,6 +61,7 @@ float current_time; }; +// this class make Robot Path to complement target value linearly class MakePath { public: @@ -108,17 +112,20 @@ int PATH[500][3]; }; +// IIC address int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; int Register[0x20] = {}; i2c master(p28, p27); BusOut LEDs(LED1, LED2, LED3, LED4); Timer timer; -JY901 jy901(&master, &timer); +JY901 jy901(&master, &timer); // gyro sensor MakePath myPath; +// this function is Robot Main Program void controlFromGcode() { + // Threshold for reaching target value float threshold_x = 5; //[mm] float threshold_y = 5; //[mm] float threshold_theta = 1 * DEG_TO_RAD; @@ -134,6 +141,7 @@ pid_y.setParameter(200.0, 0.0, 0.0); pid_theta.setParameter(100.0, 0.0, 0.0); + // read the txt file for get target position // Gコード読み取り LocalFileSystem local("local"); int array[SIZE] = {}; @@ -147,14 +155,14 @@ printf("%d, %d, %d\r\n", array[0], array[1], i); } - int row = 1; - float v[4] = {}; + int row = 1; // row of txt file + float v[4] = {}; // frequency each wheel 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); + //TextLCD lcd(p24, p26, p27, p28, p29, p30); float x_robot = 0; float y_robot = 0; @@ -173,7 +181,7 @@ while(1) { nh.spinOnce(); - // 自己位置推定 + // 自己位置推定(Estimate Self-localization) theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; for(int i = 0; i < MOTOR_NUM; i++) { @@ -185,26 +193,32 @@ x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot); y_robot += dy * cos(theta_robot) + dx * sin(theta_robot); + // caliculate error of current position // 目標位置判定 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); - lcd.printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row); + //lcd.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 Reach target position // 目標位置到達 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) { + // At first, Stop all Motor // 車輪を停止 coastAllMotor(); //pid_x.reset(); //pid_y.reset(); //pid_theta.reset(); wait_ms(500); + + // gyro reset jy901.reset(); + // update target position // Gコードを次の行に row++; if(row >= code.getGcodeSize()) @@ -217,16 +231,6 @@ nh.spinOnce(); wait_ms(1); } - //PC.printf("All task Clear\r\n"); - //PC.printf("\r\nI'm ready to start. Press Key 'a'.\r\n"); - /*char input_character = 0; - while(input_character != 'a') - { - if(PC.readable() > 0) - { - input_character = PC.getc(); - } - }*/ } jy901.reset(); @@ -235,6 +239,7 @@ v[2] = 0; v[3] = 0; + // get next target position // 目標位置把握 code.getGcode(row, ArraySize(array), array); x_desire = array[0]; @@ -244,6 +249,7 @@ path = 0; } + // caliculate velosity // 目標速度計算 else { @@ -267,6 +273,7 @@ // 本当はこうするべき // f = v * ppr / ( 2*PI * r); + // Control Motor velocity for(int i = 0; i < MOTOR_NUM; i++) { controlMotor(i, (int)v[i]); @@ -281,7 +288,11 @@ nh.initNode(); nh.subscribe(sub); coastAllMotor(); + // gyro sensor calibration + // Please wait 5 seconds... jy901.calibrateAll(5000); + + // Robot start when receive the ROS topic while(!startable) { nh.spinOnce();