Diff: main.cpp

Revision:

0:b0ca7b23bdb5

Child:

1:31f47d3fa8cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 09 08:59:22 2022 +0000
@@ -0,0 +1,191 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "pinconfig.h"
+#include "omni_wheel.h"
+#include "Servo.h"
+#include "math.h"
+#include "SerialMultiByte.h"
+#include "PID.h"
+#include "R1370.h"
+#include "OmniPosition.h"
+#include "FEP_RX22.h"
+#include "cmath"
+
+#define PI 3.141592653589
+
+DigitalIn userButton(USER_BUTTON);
+AnalogIn VOLUME(A0);
+
+
+FEP_RX22 mycon(fepTX, fepRX, fepad);
+Serial pc(pcTX, pcRX, 115200);
+Serial RS485(mdcTX,mdcRX,115200);
+
+
+DigitalOut stop(em_stop);
+ 
+SerialMultiByte rcv(sub2TX,sub2RX);
+
+ikarashiMDC motor[] = {
+    ikarashiMDC(0,0,SM,&RS485), //asi
+    ikarashiMDC(0,1,SM,&RS485), //asi
+    ikarashiMDC(0,2,SM,&RS485), //asi
+    ikarashiMDC(0,3,SM,&RS485), //asi
+    ikarashiMDC(1,0,SM,&RS485), //全体昇降1
+    ikarashiMDC(1,1,SM,&RS485), //全体昇降2
+    ikarashiMDC(1,2,SM,&RS485), //井上左昇降
+    ikarashiMDC(1,3,SM,&RS485), //井上右昇降
+    ikarashiMDC(2,0,SM,&RS485),
+    ikarashiMDC(2,1,SM,&RS485),
+    ikarashiMDC(2,2,SM,&RS485),
+    ikarashiMDC(2,3,SM,&RS485),
+};
+
+Servo pwm_imput1(BLDC1);//ブラシレス宣言
+Servo pwm_imput2(BLDC2);
+Servo pwm_imput3(BLDC3);
+
+OmniWheel omni(4);//足回り宣言
+OmniPosition posi(sub1TX, sub1RX);
+
+Timer t;
+
+int main() 
+{
+    t.start();
+    
+    double bldcspeed = 0.8;
+    
+//    double spin_power;       //足回り宣言
+//    double posiX , posiY , posiTheta;
+    
+    rcv.setHeaders(0xff,0xff);
+    rcv.startReceive(4); //SerialMultiByte受信
+    
+    mycon.StartReceive(); //コントローラー受信・宣言
+    uint8_t b[16];
+    int16_t stick[4];
+    int16_t trigger[4];
+    int16_t volume[3];
+    uint8_t toggle[4];
+    uint8_t timeout;
+    uint8_t data[128];
+    int pw;
+    
+    double speed[12] = {0};
+    
+    int16_t angle[4] = {0};//エンコーダ受信格納
+    uint8_t pulse[8] = {0};
+    
+    while(1)
+        {
+        stop = toggle[0];
+        
+        rcv.getData(pulse); //エンコーダ受信
+        for(int i=0,j=0;i<4;i++,j+=2){
+            angle[i] = pulse[j] << 8 + pulse[j+1];  
+        }
+        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);            
+        pc.printf("\r\n");
+         
+#if ControllerMode   
+        for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
+        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
+        for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
+        
+//        for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
+//        pc.printf(" | ");
+//        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
+//        pc.printf(" | ");
+//        for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
+//        pc.printf(" | ");
+#else
+        mycon.getData(data);
+        for (int i=0, tmp=1; i<8; i++) {
+            pw = pow((float)2,i);
+            b[i] = (int)((data[0] & tmp)/pw);
+            pc.printf("%d ", b[i]);
+            tmp *= 2;
+        }
+        for (int i=8, tmp=1, j=0; i<16; i++, j++) {
+            pw = pow((float)2,j);
+            b[i] = (int)((data[1] & tmp)/pw);
+            pc.printf("%d ", b[i]);
+            tmp *= 2;
+        }
+        pc.printf(" | ");
+        
+        for (int i=0; i<4; i++) {
+            stick[i] = data[i+2];
+            pc.printf("%3d ", stick[i]);
+        }
+        pc.printf(" | ");
+        
+        for (int i=0; i<2; i++) {
+            trigger[i] = data[i+6];
+            pc.printf("%3d ", trigger[i]);
+        }
+        pc.printf(" | ");
+        
+        for (int i=0; i<3; i++) {
+            volume[i] = data[i+9];
+            pc.printf("%3d ", volume[i]);
+        }
+        pc.printf(" | ");
+        
+        for (int i=0; i<4; i++) {
+            toggle[i] = data[i+12];
+            pc.printf("%3d ", toggle[i]);
+        }
+        pc.printf(" | ");
+        
+        timeout = data[8];
+        pc.printf("%3d ", timeout);
+        pc.printf(" | ");
+        
+        
+#endif
+        if (mycon.getStatus()) pc.printf("receive");
+        else pc.printf("anything error..."); 
+    
+
+        //ブラシレスモーター
+        pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
+
+        pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
+
+        pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
+//        pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
+//                  ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
+        
+        /*展開昇降*/
+        if(b[5] != 0){
+            speed[4] = 0.3;
+            speed[5] = 0.3;
+        }else if(b[4] != 0){
+            speed[4] = -0.3;
+            speed[5] = -0.3;
+        }else{
+            speed[4] = 0;
+            speed[5] = 0;
+        }
+        //機構昇降
+        if(b[9] != 0){
+            speed[6] = 0.3;
+        }else if(b[11] != 0){
+            speed[7] = 0.3;
+        }else if(b[13] != 0){    
+            speed[6] = -0.3;
+        }else if(b[14] != 0){    
+            speed[7] = -0.3;
+        }else{
+            speed[6] = 0;
+            speed[7] = 0;
+        }
+        
+        for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
+        
+        
+    }
+        
+}        
\ No newline at end of file