Revision 5:a5dc3090ba44, committed 2019-12-18

Comitter:

sgrsn

Date:

Wed Dec 18 02:51:29 2019 +0000

Branch:

StartFromROS

Parent:

4:25ab7216447f

Child:

6:55e60b9d7ff1

Commit message:

Add comment

Changed in this revision

--- 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();