Diff: main.cpp

Revision:

8:e1f1a91e9353

Parent:

7:76790bcece4b

Child:

9:88f6351221ed

--- a/main.cpp	Wed Oct 12 00:22:35 2022 +0000
+++ b/main.cpp	Wed Oct 12 16:05:05 2022 +0000
@@ -16,7 +16,6 @@
 QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
 QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
 PID front(2.0, 0, 0.001, 0.01);
-//PID rink(2.0, 0, 0.001, 0.01);
 DigitalOut stop(stop_pin);
 DigitalOut led(LED2);
 
@@ -46,11 +45,12 @@
         for (int i=0; i<8; i++) b[i]=0;
         for (int i=0; i<4; i++) stick[i]=0;
     }
-    canTR();
-//    for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
-//    pc.printf(" | ");
-//    if (mycon.getStatus()) pc.printf("received\r\n");
-//    else pc.printf("anything error...\r\n");
+//    canTR();
+    /*
+    for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
+    pc.printf(" | ");
+    if (mycon.getStatus()) pc.printf("received\r\n");
+    else pc.printf("anything error...\r\n");*/
 }
 
 void updateenc(){
@@ -58,7 +58,7 @@
     encoderValue[4] = (float)enc2.getPulses();
     encoderValue[5] = (float)enc3.getPulses();
     encoderValue[6] = (float)enc4.getPulses();
-    canTR();
+//    canTR();
 //    for (int i=0; i<4; i++) pc.printf("%d  ", encoderValue[i]);
 //    pc.printf("\r\n ");
 }
@@ -66,7 +66,7 @@
 void allanglemove(double one,double two,double three,double four){
     if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
         for(int i=0; i < 4; i++){
-            motorSpeed[i+8] = sin((atan2((double)stick[1],(double)stick[0])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
+            motorSpeed[i+8] = sin((atan2((double)stick[0],(double)stick[1])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
         }
     } else {
         motorSpeed[8]=one;
@@ -74,7 +74,7 @@
         motorSpeed[10]=three;
         motorSpeed[11]=four;
     }
-    canTR();
+//    canTR();
 }
 
 void PIDset(){
@@ -83,13 +83,6 @@
     front.setBias(0);
     front.setMode (1);
     front.setSetPoint(0);
-    /*
-    rink.setInputLimits (-180,180);
-    rink.setOutputLimits (-1,1);
-    rink.setBias(0);
-    rink.setMode (1);
-    rink.setSetPoint(0);
-    */
 }
 
 void addPID(){
@@ -97,46 +90,42 @@
     for(int i=8; i < 12; i++){
         motorSpeed[i]-=front.compute();
     }
-    canTR();
+//    canTR();
 }
-/*
-void hosoki(double m8, double m9, double m10, double m11){
-    motorSpeed[8]=m8;
-    motorSpeed[9]=m9;
-    motorSpeed[10]=m10;
-    motorSpeed[11]=m11;
-} 
 
-void seki(double m12, double m13, double m14){
-    motorSpeed[12]=m12;
-    motorSpeed[13]=m13;
-    motorSpeed[14]=m14;
-    canTR();
-}      
-*/
 int main()
 {
     mycon.StartReceive();
+    recieveController();
     stop = 1;
     led = 1;
     
-    int currentMode=1;
-    int bc=0,bn=0;
-    int sa=0;
+//    int currentMode=1;
+//    int bc=0,bn=0;
+//    int sa=0;
     
     printf("success!\r\n");
     
-    canAllReset();   
+    for(int i=0; i < 8; i++){
+        motor[i].setSpeed(0);
+    }
+    
+//    canAllReset();   
     PIDset();
     SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
-    seki.stopAll();
-//    HOSOKIkikou hosoki(&motor[10],&motor[11], &motor[8], &motor[9], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+//    seki.stopAll();
+//    SEKIkikou seki(&motor[6], &motor[4], &motor[5], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+    
+    
+    
     
     while(1) {
         recieveController();
+        canTR();
         updateenc();
+        frontAngle=jyroValue;
         
-        
+        /*
         bc=b[7]-bn;
         bn=b[7];
         
@@ -144,6 +133,7 @@
             if(bc==1){
                 currentMode=2;
                 seki.init(&motor[6],&motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
+                seki.runAll(0,0,0);
                 allanglemove(0,0,0,0);
             }else if(b[0]){
                 allanglemove(-1,-1,1,1);
@@ -163,6 +153,7 @@
                     frontAngle-=360;
                 }
                 allanglemove(0,0,0,0);
+                seki.runAll(0,0,0);
             }else if(b[5]){
                 sa=0;
                 frontAngle=jyroValue;
@@ -175,6 +166,7 @@
             if(bc==1){
                 currentMode=3;
                 seki.stopAll();
+                seki.runAll(0,0,0);
             }else{
                 seki.runAll(-0.3,0.3,0.3);
             }
@@ -184,33 +176,58 @@
             if(bc==1){
                 currentMode=1;
                 seki.stopAll();
+                seki.runAll(0,0,0);
             }
+            seki.runAll(0,0,0);*/
             allanglemove(0,0,0,0);
             addPID();
+//        }
+        
+        if(b[5]&&b[6]){
+            motorSpeed[15]=0;
+        }else if(b[5]){
+            motorSpeed[15]=0.5;
+        }else if(b[6]){
+            motorSpeed[15]=-0.5;
+        }else{
+            motorSpeed[15]=0;
+        }
+        
+        
+        if(b[7]==1){
+            seki.runAll(0.1,0.3,-0.3);
+        }else{
+            seki.runAll(-0.8,0.3,0.3);
         }
         
         for(int i=0; i < 4; i++){
             motor[i].setSpeed(motorSpeed[i+8]*0.8);
         }
         
-        for(int i=0; i < 8; i++){
-            printf("%d  ",b[i]);
+        motor[7].setSpeed(motorSpeed[15]);
+        
+        pc.printf("|wheel:  ");
+        for(int i=8; i < 12; i++){
+            pc.printf("%.2f  ",motorSpeed[i]);
         }
-        printf("|  ");
-        for(int i=0; i < 4; i++){
-            printf("%d  ",stick[i]);
+        pc.printf("|spin:  %.2f  ",motorSpeed[15]);
+//        pc.printf("|updown:  %.2f  ",motorSpeed[2]);
+        pc.printf("|enc:  ");
+        for(int i=0; i < 7; i++){
+            pc.printf("%d  ",encoderValue[i]);
+            
         }
-        printf("|  ");
-        for(int i=2; i < 15; i++){
-            printf("%.3f  ",motorSpeed[i]);
+        pc.printf("|  ");
+        pc.printf("R1370:%.3f",jyroValue);
+        pc.printf("|  ");
+        for(int i=0; i < 8; i++){
+            pc.printf("%d ",b[i]);
         }
-        printf("|  ");
-        for(int i=0; i < 12; i++){
-            printf("%d  ",encoderValue[i]);
+        pc.printf("|  ");
+        for(int i=0; i < 4; i++){
+            pc.printf("%d  ",stick[i]);
         }
-        printf("|  ");
-        printf("%.4f\r\n",jyroValue);
+        pc.printf("\r\n");
         
-        canTR();
     }
 }
\ No newline at end of file