Diff: main.cpp

Revision:

2:c040883ea536

Parent:

1:7b0db5ea0ab7

Child:

3:012e5d7bbfd5

--- a/main.cpp	Tue Jul 09 07:47:43 2019 +0000
+++ b/main.cpp	Tue Jul 09 15:57:00 2019 +0000
@@ -52,7 +52,8 @@
 RotaryInc* Speed[Moter_NUM];
 RotaryInc* Place[Moter_NUM];
 GY521 *gy;
-bool Ok = false;
+bool ok = false;
+bool able_move = true;
 double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ
 double Vx = 0,Vy = 0,Omega = 0;//速度
 double driveS[Moter_NUM],nowV[Moter_NUM];
@@ -67,7 +68,6 @@
     Vx = 0;
     Vy = 0;
     Omega = 0;
-    automove = false;
     for(int i = 0;i < Moter_NUM;i++){
         Moter[i][0]->write(0);
         Moter[i][1]->write(0);
@@ -76,8 +76,8 @@
 }
 
 void Reset(){
-    if(!Ok){
-        Ok = true;
+    if(!ok){
+        ok = true;
     }else{
         led.write(0);
         safe();
@@ -140,41 +140,29 @@
 }
 
 void getData(const std_msgs::Float32MultiArray &msgs){
-    if(!Ok && (int)msgs.data[0] != -1)return;
+    if(!ok && (int)msgs.data[0] != -1)return;
     switch((int)msgs.data[0]){
         case -1:
-            ablemove = true;
+            able_move = true;
             Reset();
             break;
         case 0:
-            ablemove = true;
+            able_move = true;
             Vx = msgs.data[1];
             Vy = msgs.data[2];
             Omega = msgs.data[3];
             break;
         case 1:
-            ablemove = true;
-            goal_x = msgs.data[1];
-            goal_y = msgs.data[2];
-            goal_yaw = msgs.data[3];
-            automove = true;
-            errer_x = 0;
-            errer_y = 0;
-            errer_omega = 0;
-            autotimer.reset();
-            autotimer.start();
+            able_move = true;
+            safe();
             break;
         case 2:
-            ablemove = true;
-            safe();
-            break;
-        case 3:
-            ablemove = false;
+            able_move = false;
             Drive(0,msgs.data[1]/255);
             Drive(1,msgs.data[2]/255);
             break;
-        case 4:
-            ablemove = false;
+        case 3:
+            able_move = false;
             Drive(2,msgs.data[1]/255);
             Drive(3,msgs.data[2]/255);
             break;
@@ -197,8 +185,6 @@
     double diff[Moter_NUM],Pspeed[Moter_NUM];
     double nowVx,nowVy,nowVt;
     double cos_yaw_2,sin_yaw_2;
-    double diff_x,diff_y,diff_yaw;
-    double now_t,use_amax;
     for(i = 0;i < Moter_NUM;i++){
         Led[i] = new DigitalOut(pwm_pin[i][2]);
         Moter[i][0] = new PwmOut(pwm_pin[i][0]);
@@ -211,7 +197,7 @@
     I2C i2c(PB_3,PB_10);
     Timer loop;
     loop.start();
-    while(button && !Ok){
+    while(button && !ok){
         nh.spinOnce();
         if(loop.read_ms() > 250){
             led = !led;
@@ -219,13 +205,13 @@
         }
     }
     led.write(0);
-    Ok = true;
+    ok = true;
     GY521 gyro(i2c);
     gy = &gyro;
     motertimer.start();
     led.write(1);
     while(true){
-        if(ablemove){
+        if(able_move){
             move();
         }else{
             for(i = 0;i<4;i++){