kouhei umezawa / Mbed 2 deprecated 2022_NHK_B_UK

2022 NHK Bteam main totyuu

Dependencies:   HOSOKIkikou FEP_RX22 QEI R1370 ikarashiMDC_2byte_ver mbed 2022_NHK_B_canTR PID SEKIkikou

Files at this revision

API Documentation at this revision

Revision 7:76790bcece4b, committed 2022-10-12

Comitter:
umekou
Date:
Wed Oct 12 00:22:35 2022 +0000
Parent:
6:e7d542a2e49e
Child:
8:e1f1a91e9353
Commit message:
jikken mada

Changed in this revision

HOSOKIkikou.lib Show annotated file Show diff for this revision Revisions of this file
SEKIkikou.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
pinconfig.h Show annotated file Show diff for this revision Revisions of this file 
--- a/HOSOKIkikou.lib	Mon Oct 10 09:54:45 2022 +0000
+++ b/HOSOKIkikou.lib	Wed Oct 12 00:22:35 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/HOSOKIkikou/#2e7a4f14e9f3
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/HOSOKIkikou/#4f901bdb470f

--- a/SEKIkikou.lib	Mon Oct 10 09:54:45 2022 +0000
+++ b/SEKIkikou.lib	Wed Oct 12 00:22:35 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SEKIkikou/#d3814c6e3694
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SEKIkikou/#3745c060228e

--- a/main.cpp	Mon Oct 10 09:54:45 2022 +0000
+++ b/main.cpp	Wed Oct 12 00:22:35 2022 +0000
@@ -5,18 +5,18 @@
 #include "QEI.h"
 #include "PID.h"
 #include "can_tr.h"
-//#include "SEKIkikou.h"
+#include "SEKIkikou.h"
 //#include "HOSOKIkikou.h"
 
 FEP_RX22 mycon(fepTX, fepRX, fepad);
 Serial pc(USBTX, USBRX, 115200);
 Serial serial(motorTX, motorRX, 115200);
-QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
+//QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
 QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
 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);
+//PID rink(2.0, 0, 0.001, 0.01);
 DigitalOut stop(stop_pin);
 DigitalOut led(LED2);
 
@@ -28,15 +28,7 @@
     ikarashiMDC(1,0,SM,&serial),
     ikarashiMDC(1,1,SM,&serial),
     ikarashiMDC(1,2,SM,&serial),
-    ikarashiMDC(1,3,SM,&serial),
-    ikarashiMDC(2,0,SM,&serial),
-    ikarashiMDC(2,1,SM,&serial),
-    ikarashiMDC(2,2,SM,&serial),
-    ikarashiMDC(2,3,SM,&serial),
-    ikarashiMDC(3,0,SM,&serial),
-    ikarashiMDC(3,1,SM,&serial),
-    ikarashiMDC(3,2,SM,&serial),
-    ikarashiMDC(3,3,SM,&serial)
+    ikarashiMDC(1,3,SM,&serial)
 };
 
 uint8_t data[128];
@@ -62,10 +54,10 @@
 }
 
 void updateenc(){
-    encoderValue[0] = (float)enc1.getPulses();
-    encoderValue[1] = (float)enc2.getPulses();
-    encoderValue[2] = (float)enc3.getPulses();
-    encoderValue[3] = (float)enc4.getPulses();
+//    encoderValue[0] = (float)enc1.getPulses();
+    encoderValue[4] = (float)enc2.getPulses();
+    encoderValue[5] = (float)enc3.getPulses();
+    encoderValue[6] = (float)enc4.getPulses();
     canTR();
 //    for (int i=0; i<4; i++) pc.printf("%d  ", encoderValue[i]);
 //    pc.printf("\r\n ");
@@ -73,14 +65,14 @@
 
 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=4; i < 8; i++){
-            motorSpeed[i] = sin((atan2((double)stick[1],(double)stick[0])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
+        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));
         }
     } else {
-        motorSpeed[4]=one;
-        motorSpeed[5]=two;
-        motorSpeed[6]=three;
-        motorSpeed[7]=four;
+        motorSpeed[8]=one;
+        motorSpeed[9]=two;
+        motorSpeed[10]=three;
+        motorSpeed[11]=four;
     }
     canTR();
 }
@@ -91,21 +83,23 @@
     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(){
     front.setProcessValue (frontAngle);
-    for(int i=4; i < 8; i++){
+    for(int i=8; i < 12; i++){
         motorSpeed[i]-=front.compute();
     }
     canTR();
 }
-
+/*
 void hosoki(double m8, double m9, double m10, double m11){
     motorSpeed[8]=m8;
     motorSpeed[9]=m9;
@@ -119,7 +113,7 @@
     motorSpeed[14]=m14;
     canTR();
 }      
-
+*/
 int main()
 {
     mycon.StartReceive();
@@ -134,7 +128,8 @@
     
     canAllReset();   
     PIDset();
-//    SEKIkikou seki(&motor[12], &motor[13], &motor[14], &b[0], &b[1], &b[2], &b[3], &env[0], &env[1], &env[2]);
+    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);
     
     while(1) {
@@ -148,7 +143,7 @@
         if(currentMode==1){
             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]);
                 allanglemove(0,0,0,0);
             }else if(b[0]){
                 allanglemove(-1,-1,1,1);
@@ -179,90 +174,23 @@
         }else if(currentMode==2){
             if(bc==1){
                 currentMode=3;
-                seki(0,0,0);
-//                hosoki.init(&motor[10],&motor[11], &motor[8], &motor[9], &b[4], &b[0], &b[2], &b[4], NULL, NULL, NULL);
-            }else if(b[0]){
-                seki(0.5,0,0);
-            }else if(b[1]){
-                seki(0,0.5,0);
-            }else if(b[2]){
-                seki(-0.5,0,0);
-            }else if(b[3]){
-                seki(0,-0.5,0);
-            }else if(b[4]){
-                seki(0,0,0.5);
-            }else if(b[5]){
-                rink.setProcessValue(encoderValue[4]);
-                seki(0,0,rink.compute());
-            }else if(b[6]){
-                seki(0,0,-0.5);
+                seki.stopAll();
             }else{
-                seki(0,0,0);
+                seki.runAll(-0.3,0.3,0.3);
             }
             allanglemove(0,0,0,0);
             addPID();
         }else if(currentMode==3){
-            
             if(bc==1){
                 currentMode=1;
-                hosoki(0,0,0,0);
-            /*
-                hosoki.stopAll();
+                seki.stopAll();
             }
-            */
-            }else if(b[0]){
-                hosoki(0.5,0,0,0);
-            }else if(b[1]){
-                hosoki(0,0.5,0,0);
-            }else if(b[2]){
-                hosoki(-0.5,0,0,0);
-            }else if(b[3]){
-                hosoki(0,-0.5,0,0);
-            }else if(b[4]){
-                hosoki(0,0,0.8,-0.8);
-            }else if(b[5]){
-                hosoki(0,0,0.9,0.9);
-            }else if(b[6]){
-                hosoki(0,0,1.0,1.0);
-            }else{
-                hosoki(0,0,0,0);
-            }
-            
-//            hosoki.runAll(0.9,-0.9,0.3,0.3);
             allanglemove(0,0,0,0);
             addPID();
         }
         
-        if(stick[2]>100||stick[2]<-100||stick[3]>100||stick[3]<-100){//機構回転
-            if(stick[2]>100){
-                motorSpeed[1] = 0.8;
-            }else if(stick[2]<-100){
-                motorSpeed[1] = -0.8;
-            }
-            if(stick[3]>100){
-                motorSpeed[2] = 0.8;
-            }else if(stick[3]<-100){
-                motorSpeed[2] = -0.8;
-            }
-        } else {
-            motorSpeed[2]=0;
-            motorSpeed[1]=0;
-        }
-        
-        for(int i=1; i < 3; i++){
-            motor[i].setSpeed(motorSpeed[i]);
-        }
-        
-        for(int i=0; i < 8; i++){
-            motor[i].setSpeed(motorSpeed[i]*0.8);
-        }
-        
-        for(int i=8; i < 12; i++){
-            motor[i].setSpeed(motorSpeed[i]);
-        }
-        
-        for(int i=12; i < 15; i++){
-            motor[i].setSpeed(motorSpeed[i]);
+        for(int i=0; i < 4; i++){
+            motor[i].setSpeed(motorSpeed[i+8]*0.8);
         }
         
         for(int i=0; i < 8; i++){

--- a/pinconfig.h	Mon Oct 10 09:54:45 2022 +0000
+++ b/pinconfig.h	Wed Oct 12 00:22:35 2022 +0000
@@ -19,9 +19,10 @@
 static PinName const motorRX = PC_11;
 
 /*encoder*/
+/*
 static PinName const encoder1_A = PB_2;
 static PinName const encoder1_B = PB_1;
-
+*/
 static PinName const encoder2_A = PA_6;
 static PinName const encoder2_B = PA_7;