Diff: states.cpp

Revision:

4:eafd5a0c3351

Parent:

3:2b909340be04

Child:

5:3273db2411d8

Child:

6:3cb78320d4db

--- a/states.cpp	Thu Jul 11 09:19:10 2019 +0000
+++ b/states.cpp	Mon Sep 09 04:48:16 2019 +0000
@@ -2,47 +2,54 @@
 
 States::States():
     anglePID(KP, KI, KD,0.001),
-    pad(padTX,padRX,ADDRESS),
+    pad(padTX,padRX),
     gyro(GYROTX,GYRORX),
     serial(MDTX,MDRX,MDBAUD),
     omni(&serial),
-    emStop(STOP)
+    emStop(STOP),
+    mechanism(&serial)
 {
     anglePID.setMode(AUTO_MODE);
     anglePID.setInputLimits(-360, 360);
     anglePID.setOutputLimits(minLimit, maxLimit);
     anglePID.setSetPoint(0.0);
     anglePID.setBias(0.0);
-
     state = 400;
+    check = 0;
     emStop = false;
 } 
 
-
 void States::tellAngle(){
     gyro.update();
     nowAngle = gyro.getAngle();
     deviation = nowAngle - idealAngle;
     if(deviation > 180) deviation -= 360;
     if(deviation < -180) deviation += 360;
-
     anglePID.setProcessValue(deviation);
 }
 
 void States::tellPad(){
     for(int i=0; i<13; i++){
-        b[i] = 1 - pad.getButton1(i);
+        b[i] = pad.getButton(i);
         b3[i] = b2[i] - b[i];
         b2[i] = b[i];
+        check += b[i]; 
     }
-    stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180));
-    norm = pad.getNorm(0);
-    if(b[1] && norm) norm = 0.25;
-    X = norm * cos(stickRad);
-    Y = norm * sin(stickRad);
-    if(b3[6] == 1) state++;
-    if(b3[0] == 1) state--;
-    
+    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]);
+    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.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--;
     /*
     *move.tellState(方角)でロボの方角
     * "front" or 0 　で前
@@ -55,21 +62,39 @@
         case 2: idealAngle = back; break;
         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()
 {
-    if(pad.receiveState() == 0){
+    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;
+        emStop = false;
     }else{
-        if(b[12]) emStop = true;
-        if(b[11]) emStop = false;
         tellAngle();
-        tellPad();
         turnPower = anglePID.compute();
-        omni.move(X, Y, -turnPower);
+        if(norm[0] && norm[0] < 200) omni.stop();
+        else omni.move(stick[0], stick[1], -turnPower);
     }
-}
+}
\ No newline at end of file