WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
Diff: main.cpp
- Revision:
- 2:83fa142c5bcc
- Parent:
- 1:f102831401a8
- Child:
- 3:4ac32aff309c
--- a/main.cpp Tue Dec 17 04:41:19 2019 +0000 +++ b/main.cpp Tue Dec 17 07:23:55 2019 +0000 @@ -113,7 +113,7 @@ { float threshold_x = 5; //[mm] float threshold_y = 5; //[mm] - float threshold_theta = 5 * DEG_TO_RAD; + float threshold_theta = 1 * DEG_TO_RAD; // 角度補正係数 float L = 233; //[mm] @@ -167,8 +167,8 @@ { 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; + 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); @@ -248,10 +248,14 @@ jy901.calibrateAll(5000); //controlFromWASD(); PC.printf("\r\nI'm ready to start. Press Enter\r\n"); - bool startable = false; - while(!startable) + //bool startable = false; + char input_character = 0; + while(input_character != 'a') { - startable = PC.readable() > 0; + if(PC.readable() > 0) + { + input_character = PC.getc(); + } } controlFromGcode(); }