Diff: states.cpp

Revision:

0:8d012d469eb4

Child:

1:38dd7564c15f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/states.cpp	Mon Jul 08 08:45:10 2019 +0000
@@ -0,0 +1,70 @@
+#include "states.h"
+
+States::States():
+    anglePID(KP, KI, KD,0.001),
+    pad(padTX,padRX,ADDRESS),
+    gyro(GYROTX,GYRORX),
+    serial(MDTX,MDRX,MDBAUD),
+    omni(&serial)
+{
+    anglePID.setMode(AUTO_MODE);
+    anglePID.setInputLimits(-360, 360);
+    anglePID.setOutputLimits(minLimit, maxLimit);
+    anglePID.setSetPoint(0.0);
+    anglePID.setBias(0.0);
+
+    state = 400;
+} 
+
+
+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);
+        b3[i] = b2[i] - b[i];
+        b2[i] = b[i];
+    }
+    stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180));
+    norm = pad.getNorm(0);
+    X = norm * cos(stickRad);
+    Y = norm * sin(stickRad);
+    if(b3[0] == 1) state++;
+    if(b3[1] == 1) state--;
+    
+    /*
+    *move.tellState(方角)でロボの方角
+    * "front" or 0 　で前
+    * "right" or 90　で右
+    * "back"  or 180 で後ろ
+    * "left" or -90 で左
+    */
+    switch(state % 4){
+        case 1: idealAngle = right; break;
+        case 2: idealAngle = back; break;
+        case 3: idealAngle = left; break;
+        default: idealAngle = front; break;
+        }
+}
+
+void States::move()
+{
+    if(pad.receiveState() == 0){
+        /*omni.stop()は止める*/
+        omni.stop();
+    }else{
+        tellAngle();
+        tellPad();
+//        tellState();
+        turnPower = anglePID.compute();
+        omni.move(X, Y, -turnPower);
+    }
+}