WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
Diff: main.cpp
- Branch:
- StartFromROS
- Revision:
- 4:25ab7216447f
- Parent:
- 3:4ac32aff309c
- Child:
- 5:a5dc3090ba44
--- a/main.cpp Tue Dec 17 12:42:15 2019 +0000 +++ b/main.cpp Wed Dec 18 02:24:18 2019 +0000 @@ -6,8 +6,20 @@ #include "MakeSequencer.h" #include "define.h" +#include <ros.h> +#include <std_msgs/Empty.h> + #include "TextLCD.h" +// robot start when startable is true +bool startable = false; + +// for ROS +ros::NodeHandle nh; +void messageCb(const std_msgs::Empty& toggle_msg){ + startable = true; +} +ros::Subscriber<std_msgs::Empty> sub("/robot/start", &messageCb); // MakeSequencer #define SIZE 6 @@ -22,9 +34,6 @@ int controlMotor(int ch, int frequency); void coastAllMotor(); -void controlFrequencyFromTerminal(); -void serialRead(); -void controlFromWASD(); class CountWheel { @@ -102,7 +111,6 @@ 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; @@ -131,7 +139,8 @@ int array[SIZE] = {}; FILE *fp = fopen( "/local/guide1.txt", "r"); MakeSequencer code(fp); - printf("size:%d\r\n", code.getGcodeSize()); + + //printf("size:%d\r\n", code.getGcodeSize()); for(int i = 0; i < code.getGcodeSize(); i++) { code.getGcode(i,sizeof(array)/sizeof(int),array); @@ -145,7 +154,7 @@ 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; @@ -163,11 +172,8 @@ while(1) { - + nh.spinOnce(); // 自己位置推定 - //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++) { @@ -185,7 +191,7 @@ 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); + 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); // 目標位置到達 @@ -205,16 +211,22 @@ { row = 0; coastAllMotor(); - PC.printf("All task Clear\r\n"); - PC.printf("\r\nI'm ready to start. Press Key 'a'.\r\n"); - char input_character = 0; + startable = false; + while(!startable) + { + 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(); + input_character = PC.getc(); } - } + }*/ } jy901.reset(); @@ -265,21 +277,17 @@ int main() { + nh.getHardware()->setBaud(9600); + nh.initNode(); + nh.subscribe(sub); coastAllMotor(); - PC.baud(9600); - //PC.attach(serialRead); jy901.calibrateAll(5000); - //controlFromWASD(); - PC.printf("\r\nI'm ready to start. Press Key 'a'.\r\n"); - //bool startable = false; - char input_character = 0; - while(input_character != 'a') + while(!startable) { - if(PC.readable() > 0) - { - input_character = PC.getc(); - } + nh.spinOnce(); + wait_ms(1); } + // main function controlFromGcode(); } @@ -325,103 +333,4 @@ { 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); - } -} +} \ No newline at end of file