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