Diff: main.cpp

Revision:

0:e6f6feb5f802

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 09 13:24:42 2019 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "PID.h"
+#include "R1370.h"
+#include "PS3.h"
+#include "omni_wheel.h"
+#include "sheets.h"
+#define PI 3.141592653589
+
+Serial serial(PC_6, PC_7, 115200);
+Serial pc(USBTX, USBRX, 115200);
+PS3 ps3(PA_0,PA_1);
+OmniWheel omni(4);
+ikarashiMDC motor[]=
+{
+    ikarashiMDC(2,0,SM,&serial),
+    ikarashiMDC(2,1,SM,&serial),
+    ikarashiMDC(2,2,SM,&serial),
+    ikarashiMDC(2,3,SM,&serial)
+};
+R1370 gyro(PB_6,PA_10);
+PID pid(4.0, 0, 0.001, 0.01);
+DigitalOut hizyou1(PC_2);
+DigitalOut hizyou2(PC_3);
+sheets sheets(&serial);
+int state1=0,state2=0,state3=0;
+
+double sign(double a){
+    return(a>0)-(a<0);
+}
+
+int main()
+{   
+    serial.baud(115200);
+    for(int i=0;i<4;i++)motor[i].braking = true;
+
+    bool b[11]={}, b2[11]={}, b3[11]={};
+    int angle_state;
+    double rad[2], dis[2], value[3],SPe[4], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power;
+    double deviation = 0.08;
+    
+    hizyou1 = 1;
+    hizyou2 = 1;
+    
+    pid.setInputLimits(-180,180);
+    pid.setOutputLimits(-0.3,0.3);
+    pid.setBias(0);
+    pid.setMode(1);
+    pid.setSetPoint(0);
+    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
+    omni.wheel[1].setRadian(PI * 3.0 / 4.0);
+    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
+    omni.wheel[3].setRadian(PI * 7.0 / 4.0);
+    
+    while(true){
+      //PS3 値読み取り
+        int stick[4],trigger[2];
+
+        /*ボタンスイッチ*/
+        for(int i = 0; i < 12; i++) {
+            b[i] = ps3.getButton(i);
+            pc.printf("%2d", b[i] );
+        }
+        /*ジョイスティック*/
+        for(int i = 0; i < 4; i++) {
+            stick[i] = ps3.getStick(i);
+            pc.printf("%4d", stick[i] );
+        }
+        /*トリガースイッチ*/
+        for(int i = 0; i < 2; i++) {
+            trigger[i] = ps3.getTrigger(i);
+            pc.printf("%4d",trigger[i]);
+        }
+        
+    /**
+     * now_angle      : 今の角度
+     * start_angle    : 素の角度
+     * original_angle : 0,-90,90,180
+     * deviation      : 差
+     * angle_state    : 90度毎
+     * zero           : 零点合わせ
+     */
+       
+        
+        X = stick[2];
+        Y = stick[3];
+        gyro.update();
+        for (int i = 0; i < 12; i++) {
+            if(b[i] == true && b2[i] == false){
+                b3[i] = !b3[i];
+            }
+        }
+        for (int i = 0; i < 12; i++) b2[i] = b[i];
+        /**
+        * ここまでテンプル
+        */
+        start_angle = gyro.getAngle();
+            
+        if ( b3[9] == 1) angle_state++;
+        if ( b3[7] == 1) angle_state--;
+        angle_state %= 4;
+        switch( ( angle_state + 8 ) % 4 ){
+            case 1: original_angle=180;
+                    break;
+            case 2: original_angle=-90;
+                    break;
+            case 3: original_angle=0;
+                    break;
+            default : original_angle=90;
+                    break;
+        }
+        if ( b[6] ) original_angle = start_angle;
+                                
+        now_angle = start_angle - original_angle;
+            
+        if ( now_angle > 180 ) now_angle  = now_angle - 360;
+        if ( now_angle < -180 ) now_angle = now_angle + 360;
+        
+        pid.setProcessValue(now_angle);
+        /**
+         * 定型文
+        */
+        spin_power = pid.compute();
+        X = (X - 127)/127;
+        Y = (Y - 127)/127;
+        omni.computeXY(X,Y,-spin_power);
+        pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state);
+        for(int i = 0;i<4;i++){
+            SPe[i] = omni.wheel[i];
+        }
+        if(fabs(SPe[0]) < 0.08)SPe[0]=0;
+        if(fabs(SPe[1]) < 0.08)SPe[1]=0;
+        if(fabs(SPe[2]) < 0.08)SPe[2]=0;
+        if(fabs(SPe[3]) < 0.08)SPe[3]=0;
+        
+        for(int i = 0; i < 4; i++)motor[i].setSpeed(SPe[i]*0.5);
+
+        
+        //シーツ機構
+        if(b3[0] == true && b3[2] ==false && b3[3] ==false){
+            sheets.lift();
+        }
+        else if(b3[0] ==false && b3[2] ==true && b3[3] ==false){
+            sheets.descent();
+        }
+        else if(b3[0] ==false && b3[2] ==false && b3[3] ==true){
+            sheets.air_open();
+        }
+        else{
+            sheets.air_close();
+        }
+        for(int i=0;i<12;i++)pc.printf("%d ",b3[i]);
+        
+    }
+}
+        
+