Diff: states.cpp

Revision:

6:3cb78320d4db

Parent:

4:eafd5a0c3351

Child:

8:8ff6db217a17

--- a/states.cpp	Mon Sep 09 04:48:16 2019 +0000
+++ b/states.cpp	Fri Sep 13 03:01:12 2019 +0000
@@ -5,7 +5,6 @@
     pad(padTX,padRX),
     gyro(GYROTX,GYRORX),
     serial(MDTX,MDRX,MDBAUD),
-    omni(&serial),
     emStop(STOP),
     mechanism(&serial)
 {
@@ -15,7 +14,6 @@
     anglePID.setSetPoint(0.0);
     anglePID.setBias(0.0);
     state = 400;
-    check = 0;
     emStop = false;
 } 
 
@@ -29,29 +27,28 @@
 }
 
 void States::tellPad(){
-    for(int i=0; i<13; i++){
+    for(int i=0; i<12; i++){
         b[i] = pad.getButton(i);
         b3[i] = b2[i] - b[i];
-        b2[i] = b[i];
-        check += b[i]; 
+        b2[i] = b[i]; 
     }
     for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
-    stick[1] = 255-stick[1];
-    stick[3] = 255-stick[3];
-    norm[0] = hypot(stick[0],stick[1]);
-    norm[1] = hypot(stick[2],stick[3]);
+    stick[0] -= 128;
+    stick[1] = 128-stick[1];
+    stick[2] -= 128;
+    stick[3] = 128-stick[3];
+    norm[0] = hypotf(stick[0],stick[1]);
+    norm[1] = hypotf(stick[2],stick[3]);
     if(norm[0] > 20) norm[0] = 0;
     if(norm[1] > 20) norm[1] = 0;
     for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
-    omni.setBrake(trigger[0]);
-    nano.sendTrigger(trigger[0]);
+    mechanism.setBrake(trigger[0]);
+
     mechanism.getTrigger(trigger[1]);
-    rad[0] = atan2(stick[1],stick[0]) + (nowAngle * (PI / 180));
-    rad[1] = atan2(stick[2],stick[3]);
-    if(b3[4] == 1) state++;
-    if(b3[5] == 1) state--;
+    if(b3[5] == 1) state++;
+    if(b3[4] == 1) state--;
     /*
-    *move.tellState(方角)でロボの方角
+    *ロボの方角
     * "front" or 0 　で前
     * "right" or 90　で右
     * "back"  or 180 で後ろ
@@ -63,38 +60,38 @@
         case 3: idealAngle = left; break;
         default: idealAngle = front; break;
     }
-   /*昇降機構*/    
-    if(b[0] && check == 1) mechanism.up();
-    else if(b[1] && check == 1) mechanism.down();
-    /*回収機構*/
-    else if(b[6] && check == 1) mechanism.collect();
-    /*つり上げ機構*/
-    else if(b[7] && check == 1) mechanism.lift();
-    /*装填機構*/
-    else if(b[2] && check == 1) mechanism.loadRight();
-    else if(b[3]) mechanism.loadLeft();
-    /*かけ機構*/
-    else if(b[9] && check == 1) mechanism.hookRight();
-    else if(b[8] && check == 1) mechanism.hookLeft();
-    
-    else nano.sendMove(0x0f);
 }
 
 void States::move()
 {
-    tellPad();
-    if(b[10]) emStop = true;
-    if(b[11]) emStop = false;
-    nano.sendHeader();
-    if(emStop == false)nano.sendMove(0xff);
-    else if(!pad.status){
-        /*omni.stop()は止める*/
-        omni.stop();
-        emStop = false;
-    }else{
+    while(1){
+        tellPad();
         tellAngle();
-        turnPower = anglePID.compute();
-        if(norm[0] && norm[0] < 200) omni.stop();
-        else omni.move(stick[0], stick[1], -turnPower);
+        if(b[10]) emStop = true;
+        if(b[11]) emStop = false;
+        if(!pad.status){
+            /*mechanism.stop()は止める*/
+            mechanism.stop();
+            emStop = false;
+        }else{
+            turnPower = anglePID.compute();
+            if(norm[0] && norm[0] < 100) mechanism.move(0,0,-turnPower);
+            else mechanism.move(stick[0], stick[1], -turnPower);
+           /*昇降機構*/    
+            if(b[0]) {mechanism.up();}
+            else if(b[1]) {mechanism.down();}
+            /*回収機構*/
+            else if(b[6]) {mechanism.collect();}
+            /*つり上げ機構*/
+            else if(b[7]) {mechanism.lift();}
+            /*装填機構*/
+            else if(b[2]) {mechanism.loadRight();}
+            else if(b[3]) {mechanism.loadLeft();}
+            /*かけ機構*/
+            else if(b[9]) {mechanism.hookRight();}
+            else if(b[8]) {mechanism.hookLeft();}
+            
+            else mechanism.noAct();
+        }
     }
 }
\ No newline at end of file