Diff: main.cpp

Revision:

26:223339e60e00

Parent:

25:0e4817d84fd5

Child:

27:2d7a978e70ab

--- a/main.cpp	Fri Apr 16 07:03:57 2021 +0000
+++ b/main.cpp	Fri Jul 23 02:16:43 2021 +0000
@@ -11,18 +11,16 @@
 
 PID angle(2.0,0,0,0.01);
 Serial pc(USBTX, USBRX, 115200);
+SerialMultiByte arduino(PC_12,PD_2);
+SerialMultiByte m5stack(PC_10,PC_11);
 
 PwmOut beep(PA_0);
-DigitalOut led1(PA_11);
+
+DigitalIn  sw(PC_13);
+DigitalOut led1(PB_5);
 DigitalOut led2(LED2);
-SerialMultiByte arduino(PC_10,PC_11);
-
-const double PII = 3.14159265358979;
-int stick[4],trigger[2];
-int b[12],b2[12],b3[12];
-float norm[2];
-
-OmniWheel omni(3);
+DigitalOut led3(PA_1);
+DigitalOut led4(PA_10);
 
 motorSmLap motor[] = {
     motorSmLap(PC_6, PC_9, PC_8),
@@ -30,10 +28,23 @@
     motorSmLap(PA_8, PB_4, PB_7)
     };
     
+const double PII = 3.14159265358979;
+OmniWheel omni(3);
 
-void setup();
+Ticker timer;
+int setmode=0;
+void tick(void)
+{
+    //pc.printf(" %d\n",setmode);
+    if(sw.read()==0)led2=1; else led2=0;
+}
 
 void setup(){
+        arduino.setHeaders(127,127);
+        arduino.startReceive(5);
+        m5stack.setHeaders(127,127);
+        m5stack.startReceive(9);
+        
         omni.wheel[0].setRadian(PII/2.0);
         omni.wheel[1].setRadian(7.0/6.0*PII);
         omni.wheel[2].setRadian(11.0/6.0*PII);
@@ -76,79 +87,152 @@
 
 int main() {
     setup();
+    int ssw=1;
     float x, y,now_angle;
     jy.calibrateAll(50);
+    uint8_t weight[5];
+    uint8_t data[10];
     angle.setInputLimits(-180.0,180.0);
     angle.setOutputLimits(-1,1);
     angle.setBias(0);
     angle.setMode(1);
     angle.setSetPoint(0);
-    arduino.setHeaders(127,127);
-    arduino.startReceive(9);
-    uint8_t data[10];
     
+    sw.mode(PullUp);
+    timer.attach(&tick, 0.1);
+
     while (true) {
-        led2=1;
-        arduino.getData(data);
-        y=(data[3]-127.5)/127.5;
-        x=(data[2]-127.5)/127.5;
-        //誤差をなくす
-        if(x==-1 && y==-1){//
-            x=0;
-            y=0;
+        
+/*青スイッチモード切り替え*/
+        if(sw.read()==1){
+            if(ssw==0){
+                ssw=1;
             }
-        if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){
-            x=0;
+        }else{
+            if(ssw==1){
+                setmode++;
+                ssw=0;
+            }
+        if(setmode>=2){
+            setmode=0;
+            }
         }
         
-        if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){
-            y=0;
-        }
-        //十字モード
-        if(data[1]==1){
-            y=0.5;
-            }
-        if(data[1]==2){
-            y=-0.5;
-            }
-        if(data[1]==4){
-            x=0.5;
-            }
-        if(data[1]==8){
-            x=-0.5;
-            }
-        //iosジャイロモード
-        if(data[0]==10){//L1とR1が同時に押された時の値
-            if(data[8]<30){
-                y=0.5;
+        switch(setmode){
+/*RCWCcontrollerモード*/  
+            case 0:
+                led1=1;
+                led3=0;
+                led4=0;
+                m5stack.getData(data);
+                y=(data[3]-127.5)/127.5;
+                x=(data[2]-127.5)/127.5;
+                //誤差をなくす
+                if(x==-1 && y==-1){//
+                    x=0;
+                    y=0;
+                    }
+                if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){
+                    x=0;
+                }
+                
+                if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){
+                    y=0;
                 }
-            if(data[8]>230){
-                y=-0.5;
+                //十字モード
+                if(data[1]==1){
+                    y=0.5;
+                    }
+                if(data[1]==2){
+                    y=-0.5;
+                    }
+                if(data[1]==4){
+                    x=0.5;
+                    }
+                if(data[1]==8){
+                    x=-0.5;
+                    }
+                //iosジャイロモード
+                if(data[0]==10){//L1とR1が同時に押された時の値
+                    if(data[8]<30){
+                        y=0.5;
+                        }
+                    if(data[8]>230){
+                        y=-0.5;
+                        }
+                    if(data[6]<40){
+                        x=-0.5;
+                        }
+                    if(data[6]>220){
+                        x=0.5;
+                        }
+                    }
+                now_angle = jy.getZaxisAngle();
+                
+                if(now_angle>180 && now_angle<360){
+                    now_angle = now_angle-360;
                 }
-            if(data[6]<40){
-                x=-0.5;
+                angle.setProcessValue(now_angle);
+                omni.computeXY(
+                    x,
+                    y,
+                    0,
+                    0,
+                    angle.compute()
+                    
+                );
+                for(int i=0; i<3; i++){
+                    motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
                 }
-            if(data[6]>220){
-                x=0.5;
+                pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
+                break;
+                
+/*wii balance　boardモード*/  
+            case 1:
+                led1=0;
+                led3=1;
+                led4=0;
+                arduino.getData(weight);
+                for (uint8_t i = 0; i < 4; i++) {
+                    pc.printf("%d",weight[i]);
+                    pc.printf("\t");
                 }
-            }
-        now_angle = jy.getZaxisAngle();
-        
-        if(now_angle>180 && now_angle<360){
-            now_angle = now_angle-360;
+            
+                y = weight[0] + weight[2] - weight[1] - weight[3];
+                x = weight[0] + weight[1] - weight[2] - weight[3];
+                if(y < -50){
+                    y = -0.5;
+                }else if(y > 50){
+                    y = 0.5;
+                }else{
+                    y = 0;
+                }
+                if(x < -50){
+                    x = -0.5;
+                }else if(x > 50){
+                    x = 0.5;
+                }else{
+                    x = 0;
+                }
+                now_angle = jy.getZaxisAngle();
+                
+                if(now_angle>180 && now_angle<360){
+                    now_angle = now_angle-360;
+                }
+                angle.setProcessValue(now_angle);
+                omni.computeXY(
+                    x,
+                    y,
+                    0,
+                    0,
+                    angle.compute()
+                    
+                );
+                for(int i=0; i<3; i++){
+                    motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
+                }
+                pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
+                break;         
         }
-        angle.setProcessValue(now_angle);
-        omni.computeXY(
-            x,
-            y,
-            0,
-            0,
-            angle.compute()
-            
-        );
-        for(int i=0; i<3; i++){
-            motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
-        }
-        pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
     }
 }
\ No newline at end of file