Diff: main.cpp

Revision:

0:2b93017d0053

Child:

1:a6aa0b47c9ae

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 24 14:27:04 2021 +0000
@@ -0,0 +1,180 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "ikarashiSV.h"
+#include "controller.h"
+#include "pinconfig.h"
+#include "omni_wheel.h"
+#include "Servo.h"
+#include "math.h"
+#include "position_controller.h"
+#include "OmniPosition.h"
+#include "PID.h"
+
+#define PI 3.141592653589
+
+Bcon mycon(fepTX, fepRX, fepad);
+Serial pc(pcTX, pcRX, 115200);
+DigitalOut stop(em_stop);
+ikarashiSV  slvT(slv1,slv2, slv3,slv4);
+ikarashiSV2 slvU(slv5,slv6);
+Servo servo(servo1);
+//DigitalOut test3(PA_5);
+//DigitalOut test4(PB_10);
+//DigitalOut test1(PB_5);
+//DigitalOut test2(PB_4);
+
+//PID angle(2.0,0.0,0.00001,0.01);
+PID angle(10.0, 0.0, 0.001, 0.01);
+OmniWheel omni(4);
+OmniPosition posi(mwTX, mwRX);
+Serial RS485(mdcTX,mdcRX,115200);
+ikarashiMDC motor[] = {
+    ikarashiMDC(0,0,SM,&RS485),/*足*/
+    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),/*腕前後*/
+};
+
+PositionController pcon[] = {
+    PositionController(200.0, 200.0, 0.2, 0.0, 0.6),
+    PositionController(500.0, 500.0, 0.1, 0.0, 0.20),
+    PositionController(1.0, 13.0, 0.14, 0.05, 0.15),
+    PositionController(100.0, 100.0, 0.11, 0.0, 0.20),
+    PositionController(300, 400, 0.1, 0.0, 0.2),
+    PositionController(700, 1000, 0.1, 0.0, 0.25),
+    PositionController(1500.0, 1500.0, 0.1, 0.0, 0.30),
+    PositionController(200, 200, 0.1, 0.0, 0.20)
+};
+
+void add(){
+//    slv.add_state();
+}
+
+
+int main(void){
+
+    mycon.StartReceive();
+    uint8_t b[8];
+    int16_t stick[4];
+    double accele[4];
+    double speedX, speedY, speed[6];
+    double spin_power;
+    double X, Y, R;
+
+    int val;
+    long int xycnt=0, svcnt=0;
+
+    servo.calibrate(0.00095, 90);
+    servo.write(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);
+    
+    angle.setInputLimits(-3.14,3.14);
+    angle.setOutputLimits(-0.6,0.6);
+    angle.setBias(0);
+    angle.setMode(1);
+    angle.setSetPoint(0);
+    
+    while (1)
+    {
+        for(int i=0; i<6; i++){
+            speed[i] = 0;
+        }
+        
+        // 非常停止
+        stop = 1;
+        
+        // コントローラー受信
+        for (int i=0; i<8; i++) b[i] = mycon.getButton(i);     
+        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
+        
+        // 腕昇降
+        if(b[0] != 0){
+            speed[4] = 0.2;
+        }else if(b[2] != 0){
+            speed[4] = -0.2;
+        }else{
+            speed[4] = 0;
+        }
+
+        // 腕前後
+        if(b[3] != 0){
+            speed[5] = 0.2;
+        }else if(b[1] != 0){
+            speed[5] = -0.2;
+        }else{
+            speed[5] = 0;
+        }
+        
+        // 計測輪
+        X = posi.getX();
+        Y = posi.getY();
+        R = posi.getTheta();
+        
+        // PID
+        angle.setProcessValue(R);
+        spin_power = -angle.compute();
+        
+        // s字制御
+        if (xycnt==300) pcon[0].targetXY(0, -1000);
+        pcon[0].compute(X, Y);
+        spin_power = -angle.compute();  
+        omni.computeXY(pcon[0].getVelocityX(), pcon[0].getVelocityY(), spin_power);
+        for (int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
+
+        // 非常停止テスト用
+//        for (int i=0; i<4; i++) speed[i] = spin_power;
+//        for (int i=0; i<4; i++) speed[i] = 0;
+        
+        // モーター出力
+        for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
+        
+        // 以下ソレノイド
+        if ((X > -50 && X < 50) && (Y > -1050 && Y < -950)) svcnt++;
+//        svcnt++;
+        if (svcnt == 300)  slvU.solenoid(1);
+        if (svcnt == 500)  slvT.solenoid(1);
+        if (svcnt == 600)  slvT.solenoid(2);
+        if (svcnt == 800) slvT.solenoid(0);
+        if (svcnt == 1000) slvU.solenoid(0);
+        
+        // イカサーボ
+        if(b[7] != 0){
+            servo.write(0.3);
+        }else{
+            servo.write(0);
+        }
+        
+        // 表示部
+        // コントローラ
+        /*
+        if (mycon.status) {
+            pc.printf("received");
+//            for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
+            pc.printf("%d ", mycon.sum);
+            pc.printf(" | ");
+            for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
+            pc.printf(" | ");
+        }
+        else pc.printf("anything error... | ");
+        */
+        
+        // 計測輪
+        pc.printf("x: %5d | y: %5d | r: %3.2f | Vx: %f | Vy: %f "
+        , posi.getX(), posi.getY(), posi.getTheta(), pcon[0].getVelocityX(), pcon[0].getVelocityY());
+        
+        // スピード
+        pc.printf("spd: ");
+        for (int i=0; i<6; i++) pc.printf("%d: %2.2f | ", i, speed[i]);
+        
+//        pc.printf("xycnt: %d | svcnt: %d", xycnt, svcnt);
+        pc.printf("\r\n");
+        xycnt++;
+    }
+    
+}
\ No newline at end of file