Revision 2:25f1ed5834b8, committed 2021-10-31

Comitter:

piroro4560

Date:

Sun Oct 31 23:42:55 2021 +0000

Parent:

1:a6aa0b47c9ae

Commit message:

NHKRobocon 2021 Bteam

Changed in this revision

--- a/main.cpp	Fri Oct 29 23:24:04 2021 +0000
+++ b/main.cpp	Sun Oct 31 23:42:55 2021 +0000
@@ -1,4 +1,3 @@
-// 10/29 最後のプログラム
 #include "mbed.h"
 #include "ikarashiMDC.h"
 #include "ikarashiSV.h"
@@ -13,11 +12,15 @@
 
 #define PI 3.141592653589793
 
-#define movenum 2
+#define movenum 4
 
 #define EndRotate 0.1
-#define xyLimit   75
-#define DnkuNum   6
+#define xLimit   75
+#define yLimit   40
+#define Dnknum   4
+#define Velo     0.6
+#define Velos    0.3
+#define spinVelo 0.3
 
 
 Bcon mycon(fepTX, fepRX, fepad);
@@ -26,6 +29,8 @@
 ikarashiSV  slvT(slv1,slv2, slv3,slv4);
 ikarashiSV2 slvU(slv5,slv6);
 Servo servo(servo1);
+DigitalOut sv1(PC_8);
+DigitalOut sv2(PC_9);
 
 OmniWheel omni(4);
 OmniPosition posi(mwTX, mwRX);
@@ -35,31 +40,34 @@
     ikarashiMDC(0,1,SM,&RS485),/*足*/
     ikarashiMDC(0,2,SM,&RS485),/*足*/
     ikarashiMDC(0,3,SM,&RS485),/*足*/
-    ikarashiMDC(1,0,SM,&RS485),/*腕昇降*/
-    ikarashiMDC(1,1,SM,&RS485),/*腕前後*/
+    ikarashiMDC(1,0,SM,&RS485),
+    ikarashiMDC(1,2,SM,&RS485),
 };
 
 PID angle(10.0, 0.0, 0.001, 0.01);
 PositionController pcon[] = {
-    PositionController(200.0, 250.0, 0.35, 0.0, 0.6),
-    PositionController(200.0, 150.0, 0.35, 0.0, 0.8),
-    PositionController(200.0, 150.0, 0.3, 0.0, 0.6),
+    PositionController(200.0, 250.0, 0.35, 0.0, 0.8),
+    PositionController(200.0, 300.0, 0.35, 0.0, 0.8),
+    PositionController(200.0, 100.0, 0.3, 0.0, 0.8),
     PositionController(200.0, 150.0, 0.2, 0.0, 0.6),
-    PositionController(200.0, 150.0, 0.2, 0.0, 0.6),
+    PositionController(200.0, 250.0, 0.2, 0.0, 0.6),
     PositionController(200.0, 150.0, 0.2, 0.0, 0.6)
 };
 
 Timer sht;
 Timer xyt;
+Timer timeout;
 
 void Move();
 void Shoot();
 void Dunkset();
 void Dunk();
-void Waiting();
+void Waiting(bool pw, double a);
 void Manual();
+int getpn(double a);
 void Setzero();
 void Rotate(double ang);
+void slnd();
 
 uint8_t b[8], b_[8], bttn[8];
 int16_t stick[4];
@@ -94,98 +102,76 @@
 };
 */
 
-/*
+// 本番・リハ用
 double x[movenum] = {
-    2575.0, // front:2575, left:787.5, right:4362.5
-    -1800.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0
-};
-double y[movenum] = {
-    825.0,
-    632.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0
-};
-double ang[movenum] = {
-    0.0, 
-    -30*PI/180.0, 
-    0.0, 
-    0.0,
-    0.0, 
-    0.0
-};
-*/
-
-double x[movenum] = {
-    645.0, // 700
-    1915.0/*,
-    0.0,
-    0.0,
-    0.0,
+    600.0, // 700 / -15
+    1960.0,//     / +15
+    1750.0,
+    -1700.0/*,
+    0.0/*,
     0.0*/
 };
 double y[movenum] = {
-    1387.4, // 1327.4
-    -370.4/*,
-    0.0,
-    0.0,
-    0.0,
+    1307.4, // 1327.4 // +50
+    -350.4,
+    560.0, // +30
+    -560.0/*, -30
+    3723.0,
     0.0*/
 };
 double ang[movenum] = {
-    -33.5*PI/180.0,
-    0.0*PI/180.0/*,
+    33.5*PI/180.0,
     0.0,
-    0.0,
-    0.0,
+    -33.5*PI/180.0,
+    0.0/*,
+    0.0/*,
     0.0*/
 };
 
+// 右過ぎたら+ , 左過ぎたら-
 double errx[movenum] = {
-    0.0,
-    0.0*PI/180.0/*,
-    0.0,
-    0.0,
-    0.0,
+    20.0,
+    -70.0,
+    -10.0,
+    100.0/*,
+    0.0/*,
     0.0*/
 };
 
+// 奥に行き過ぎてたら+ , 手前すぎたら-
 double erry[movenum] = {
-    0.0,
-    130.0/*,
-    0.0,
-    0.0,
-    0.0,
+    -5.0, // 0, 1, 2 + 40
+    -20.0,
+    -20.0,
+    -20.0/*,
+    0.0/*,
     0.0*/
 };
 
-
+// 右に傾きすぎてたら+
 double errang[movenum] = {
     0.0,
-    2.0*PI/180.0/*
+    -1.5*PI/180.0,
     0.0,
-    0.0,
-    0.0,
+    0.0/*,
+    0.0/*,
     0.0*/
 };
     
 
-// ゴールがロボより左：ang:+, ゴールがロボより右：ang:-
+// 
 void Move(int num, bool shtor, bool dnkor)
 {
     double nowX, nowY, nowAngle;
+    double vx, vy, vr, vang, v;
     started=false;
     
-    pcon[num].targetXY(x[num], y[num]);
+    if (num<Dnknum) pcon[num].targetXY(x[num], y[num]);
     angle.setSetPoint(0);
+    
     xyt.reset();
-    zeroX = posi.getX()+errx[num];
-    zeroY = posi.getY()+erry[num];
+    zeroX = posi.getX()-errx[num];
+    zeroY = posi.getY()-erry[num];
 //    for (int i=0; i<num; i++) {
 //        zeroX += x[i];
 //        zeroY += y[i];
@@ -193,8 +179,11 @@
     prX += x[num];
     prY += y[num];
     
+    timeout.reset();
+    timeout.start();
     while(1) {
         stop = 1;
+        servo.write(0);
         nowX     = posi.getX();
         nowY     = posi.getY();
         nowAngle = posi.getTheta();
@@ -203,10 +192,32 @@
         // PID
         angle.setProcessValue(nowAngle);
         // omniwheel
-        omni.computeXY(pcon[num].getVelocityX(), pcon[num].getVelocityY(), -angle.compute());
+        omni.computeXY(pcon[num].getVelocityX(), pcon[num].getVelocityY(), -angle.compute()*(num != Dnknum));
+//        omni.computeXY(pcon[num].getVelocityX()*(-sin(vang-nowAngle), pcon[num].getVelocityY()*(z(vang-nowAngle), -angle.compute()*(num != Dnknum));
+        for (int i=0; i<8; i++) {
+            b[i] = mycon.getButton(i);
+        }
+        if (timeout.read_ms() >= 4000) {
+            for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
+            
+            if (b[0] && !b[1] && !b[2] && !b[3]) { vx = 0;     vy = Velo; }
+            if (b[1] && !b[0] && !b[2] && !b[3]) { vx = -Velo; vy = 0; }
+            if (b[2] && !b[1] && !b[0] && !b[3]) { vx = 0;     vy = -Velo; }
+            if (b[3] && !b[1] && !b[2] && !b[0]) { vx = Velo;  vy = 0; }
+            if (!b[0] && !b[1] && !b[2] && !b[3]){ vx = 0;     vy = 0; }
+            
+            if (b[5] && !b[7])  vr = spinVelo;
+            if (b[7] && !b[5])  vr = -spinVelo;
+            if (!b[7] && !b[5]) vr = 0;
+        
+            omni.computeXY(vx, vy, vr);
+        }
         // mdc
         for(int i=0; i<4; i++) motor[i].setSpeed(omni.wheel[i]);
-        for(int i=4; i<6; i++) motor[i].setSpeed(0);
+        motor[4].setSpeed(0);
+        motor[5].setSpeed(0);
+        
+        if (b[6]) Dunk();
         
         // 表示
 //        pc.printf("trgt(%5.2f,%5.2f)x:%5.1f | y:%5.1f | r:%1.3f | vx:%f | vy:%f | vr:%f  | cnt:%d \r\n"
@@ -217,37 +228,39 @@
 //        else       pc.printf("not\r\n");
         
         // 射出
-        if ( !started && nowX>(prX-75) && nowX<(prX+75) && nowY>(prY-75) && nowY<(prY+75) )
+        if ( !started && nowX>(prX-75) && nowX<(prX+75) && nowY>(prY-yLimit) && nowY<(prY+yLimit) )
         { // タイマーが動いてなければ動かす 範囲から出ればタイマーストップ    
             xyt.start();
             started = true;
         }
-        if (started && !( nowX>(prX-75) && nowX<(prX+75) && nowY>(prY-75) && nowY<(prY+75) )) 
+        if (started && !( nowX>(prX-75) && nowX<(prX+75) && nowY>(prY-yLimit) && nowY<(prY+yLimit) )) 
         {
             started = false;
             xyt.stop();
             xyt.reset();
         }
             
-        if ( dnkor && num==6 && xyt.read_ms()>=500 ) {
+        if ( dnkor && num==Dnknum && xyt.read_ms()>=500 ) {
             xyt.stop();
             xyt.reset();
             
             Dunk();
             return;
+      
         }
-        if ( shtor && num!=DnkuNum && xyt.read_ms() >= 1000) {
+        if ( shtor && xyt.read_ms() >= 650) {
             xyt.stop();
             xyt.reset();
-            Rotate(-ang[num]+errang[num]);
+            Rotate(ang[num]-errang[num]);
             
-            Shoot();
+            if (num<3) Shoot();
+            Rotate(0);
             return;
         }
-        if (!shtor && !dnkor && xyt.read_ms()>=1000) {
+        if (!shtor && !dnkor && xyt.read_ms()>=500) {
             xyt.stop();
             xyt.reset();
-            Rotate(-ang[num]);
+            Rotate(ang[num]-errang[num]);
             
             Rotate(0);
             
@@ -256,40 +269,41 @@
     }
 }
 
-void Rotate(double ang) // degree値
+void Rotate(double ang)
 {
     angle.setSetPoint(ang);
     
     while(1) {
+        servo.write(0);
         angle.setProcessValue(posi.getTheta());
         for(int i=0; i<4; i++) motor[i].setSpeed(-angle.compute());
         for(int i=4; i<4+2; i++) motor[i].setSpeed(0);
         pc.printf("rotating... | %f\r\n", posi.getTheta());
 //        if (fabs(-angle.compute()) < EndRotate) return;
-        if (fabs(ang - posi.getTheta()) < 3*PI/180.0) return;
+        if (fabs(ang - posi.getTheta()) < 2*PI/180.0) return;
     }
 }
 
 void Shoot()
 {
-    Waiting();
+    Waiting(false,0);
     sht.reset();
     sht.start();
     int shtflag=0;
     while (1) {
+        servo.write(0);
         stop=1;
         for(int i=0; i<6; i++) motor[i].setSpeed(0);
         for(int i=4; i<6; i++) motor[i].setSpeed(0);
         pc.printf("shooting... | %4d\r\n", sht.read_ms());
         if (shtflag==0 && sht.read_ms() >= 0) { slvU.solenoid(1); shtflag++; }
         if (shtflag==1 && sht.read_ms() >= 0) { slvT.solenoid(0); shtflag++; }
-        if (shtflag==2 && sht.read_ms() >= 1700) { slvT.solenoid(1); shtflag++; }
-        if (shtflag==3 && sht.read_ms() >= 2200) { slvT.solenoid(2); shtflag++; }
-        if (shtflag==4 && sht.read_ms() >= 2750) { slvU.solenoid(0); shtflag++; }
-        if (shtflag==5 && sht.read_ms() >= 2750) {
+        if (shtflag==2 && sht.read_ms() >= 1500) { slvT.solenoid(1); shtflag++; }
+        if (shtflag==3 && sht.read_ms() >= 2000) { slvT.solenoid(2); shtflag++; }
+        if (shtflag==4 && sht.read_ms() >= 2250) { slvU.solenoid(0); shtflag++; }
+        if (shtflag==5 && sht.read_ms() >= 2250) {
             sht.stop();
             sht.reset();
-            Rotate(0);
             return;
         }
 //        if (sht.read_ms() >= 1000) slvU.solenoid(1);
@@ -309,6 +323,7 @@
 void Dunkset()
 {
     if (slvU.solenoid_status2 != 0) slvU.solenoid(0);
+    if (slvT.solenoid_status  != 0) slvT.solenoid(0);
     if (slvT.solenoid_status  != 1) slvT.solenoid(1);
     return;
 }
@@ -316,44 +331,86 @@
 void Dunk()
 {
     Dunkset();
+    Waiting(true, 0.52);
     sht.reset();
     sht.start();
+    
     while (1)
     {
         stop = 1;
+        for (int i=0; i<8; i++) {
+            b[i] = mycon.getButton(i);
+        }
         for(int i=0; i<6; i++) motor[i].setSpeed(0);
         servo.write(0.52);
-        if (sht.read_ms() >= 0) slvU.solenoid(1);
-        if (sht.read_ms() >= 1500) slvT.solenoid(2);
+        if (sht.read_ms() >= 1500) slvU.solenoid(1);
+        if (sht.read_ms() >= 3000) slvT.solenoid(2);
         for(int i=0; i<6; i++) motor[i].setSpeed(0);
         pc.printf("Dunking... | time:%4d\r\n", sht.read_ms());
-        if (sht.read_ms() >= 1750) {
+        if (sht.read_ms() >= 3050) {
             sht.stop();
             sht.reset();
-            Rotate(0);
+//            Rotate(0);
             return;
         }
         
     }
 }
 
-void Waiting()
+void Waiting(bool pw, double a)
 {
     while(1) {
         stop=1;
-        
+        servo.write(a);
         for (int i=0; i<8; i++) {
             b[i] = mycon.getButton(i);
         }
-        pc.printf("waiting...\r\n");
-        for(int i=0; i<6; i++) motor[i].setSpeed(0);
+        pc.printf("waiting...");
+        for (int i=0; i<8; i++) pc.printf(" | i: %d", b[i]);
+        pc.printf(" | %5d, %5d", posi.getX(), posi.getY());
+        pc.printf("\r\n");
+        if (pw==true) {
+            // 腕昇降
+            if (b[5]==1 && b[7]==0) {
+                motor[5].setSpeed(-0.3);
+            } else if (b[5]==0 && b[7]==1) { 
+                motor[5].setSpeed(0.3);
+            } else {
+                 motor[5].setSpeed(0);
+            }
+            
+            // 旋回
+            if (b[1]==1 && b[3]==0) {
+                for(int i=0; i<4; i++) motor[i].setSpeed(0.25);
+            } else if (b[1]==0 && b[3]==1) {
+                for(int i=0; i<4; i++) motor[i].setSpeed(-0.25);
+            } else {
+                for(int i=0; i<4; i++) motor[i].setSpeed(0);
+            }
+            motor[4].setSpeed(0);
+        } else {
+            for(int i=0; i<6; i++) motor[i].setSpeed(0);
+        }
+//        if (b[6] == 1) {sv1=1;sv2=0;}
         if (b[4] == 1) return;
     }
 }
 
+void slnd(int i)
+{
+    if (i) {
+        sv1=1;
+        sv2=0;
+    } else {
+        sv1=0;
+        sv2=1;
+    }
+    return;
+}
+
 void Manual()
 {
-    double rawang, ang, dffr;
+    double vx, vy, vr;
     while (1) {
         stop=1;
         for (int i=0; i<8; i++) {
@@ -361,26 +418,40 @@
             bttn[i] = b[i]==1 && b_[i]==0;
         }
         for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
-        pc.printf("manualing... | vx:%3d, vy:%3d | %d,%d,%d,%d | X:%3.3d Y:%3.3d"
-        , stick[0], stick[1], bttn[0], bttn[1], bttn[2], bttn[3], posi.getX(), posi.getY(), posi.getTheta());
-        rawang = posi.getTheta();
-        if (b[1]) dffr += 5.0*PI/180.0;
-        if (b[3]) dffr -= 5.0*PI/180.0;
-        ang = rawang+dffr;
+        
+        if (b[0] && !b[1] && !b[2] && !b[3]) { vx = 0;     vy = Velo; }
+        if (b[1] && !b[0] && !b[2] && !b[3]) { vx = -Velo; vy = 0; }
+        if (b[2] && !b[1] && !b[0] && !b[3]) { vx = 0;     vy = -Velo; }
+        if (b[3] && !b[1] && !b[2] && !b[0]) { vx = Velo;  vy = 0; }
+        if (!b[0] && !b[1] && !b[2] && !b[3]){ vx = 0;     vy = 0; }
+        
+        if (b[5] && !b[7])  vr = spinVelo;
+        if (b[7] && !b[5])  vr = -spinVelo;
+        if (!b[7] && !b[5]) vr = 0;
+        
+        servo.write(0.52);
+        
+        pc.printf("manualing... | vx:%3d, vy:%3d | %d,%d,%d,%d | X:%5d Y:%5d R:%f \r\n"
+        , vx, vy, bttn[0], bttn[1], bttn[2], bttn[3], posi.getX(), posi.getY(), posi.getTheta());
 
         // PID
-        angle.setProcessValue(ang);
+//        angle.setProcessValue(ang);
         // omniwheel
-        omni.computeXY(stick[0]/128.0, stick[1]/128.0, -angle.compute());
+//        omni.computeXY(stick[0]/128.0, stick[1]/128.0, -angle.compute());
+        omni.computeXY(vx, vy, vr);
+        
         // mdc
+//        for(int i=0; i<4; i++) motor[i].setSpeed(output[i]);
         for(int i=0; i<4; i++) motor[i].setSpeed(omni.wheel[i]);
-        for(int i=4; i<6; i++) motor[i].setSpeed(0);
-        if (b[0]) Shoot();
-        if (b[2]) Dunk();
+        motor[4].setSpeed(0);
+        motor[5].setSpeed(0);
+        if (b[6]) Dunk();
         for (int i=0; i<8; i++) b_[i] = b[i];
     }
 }
 
+int getpn(double a) { return ((a>0) - (a<=0)); }
+
 void Setzero(int n)
 {
     zeroX = prX;
@@ -418,13 +489,19 @@
     }
     */
     
+//    slnd(1);
     
     for (int i=0; i<movenum; i++) {
 //    for (;;) {
-        Waiting();
-        Move(i, true, false); // 動きの番号、シュートの有無、ダンクの有無
+        Waiting(true, 0);
+        Move(i, true, true); // 動きの番号、シュートの有無、ダンクの有無
         // シュートテスト
 //        Shoot();
     }
+    Manual();
+    Waiting(true, 0);
+//    Rotate(PI);
     
-}   
\ No newline at end of file
+    
+    
+}
\ No newline at end of file