Diff: main.cpp

Revision:

0:187c4874bf41

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 21 03:15:45 2020 +0000
@@ -0,0 +1,219 @@
+#include "mbed.h"
+#include "EC.h"
+#include "SpeedController.h"
+#define RESOLUTION 500
+
+DigitalOut led(LED1);
+CAN can1(PB_5,PB_13);
+/*
+0 start/stop
+1 to 4 , 6 to 9
+5 stop
+10 clock move
+11 not clock move
+12 speed change
+*/
+Ec2multi ec[]= {
+    Ec2multi(PC_5,PB_2,RESOLUTION),
+    Ec2multi(PA_11,PB_1,RESOLUTION),
+    Ec2multi(PB_12,PB_15,RESOLUTION),
+    Ec2multi(PC_4,PB_14,RESOLUTION)
+};  //2逓倍用class
+
+Ec2multi ecXY[]= {
+    Ec2multi(PC_6,PB_8,RESOLUTION),
+    Ec2multi(PC_8,PB_9,RESOLUTION)
+};
+
+SpeedControl motor[]= {
+    SpeedControl(PA_5,PC_7,50,ec[0]),
+    SpeedControl(PC_9,PA_1,50,ec[1]),
+    SpeedControl(PA_10,PB_4,50,ec[2]),
+    SpeedControl(PA_9,PA_7,50,ec[3])
+};
+
+DigitalIn button(USER_BUTTON);
+
+class CAN_ticker
+{
+public:
+    CAN_ticker():x(0)
+    {
+        data[0]=0;
+    }
+    void canmsg_read()
+    {
+        CANMessage msg;
+        if(can1.read(msg)) {
+            if(msg.id == 1) {
+                x=(short)(msg.data[0]);
+            }
+        }
+    }
+    int get_xCAN()
+    {
+        return x;
+    }
+private:
+    char data[0];
+    int x;
+};
+//手動出力
+double canOmega[4]= {
+    0,0,0,0
+};
+void ledturn()
+{
+    led=1;
+    wait(0.04);
+    led=0;
+}
+void calOmega_CAN(int canx)
+{
+    static double a=0.1;
+    static int count=0;
+    switch(canx) {
+        case 1:
+            canOmega[0]=0;
+            canOmega[1]=a;
+            canOmega[2]=0;
+            canOmega[3]=-a;
+            ledturn();
+            break;
+        case 2:
+            canOmega[0]=a*1.41;
+            canOmega[1]=a*1.41;
+            canOmega[2]=-a*1.41;
+            canOmega[3]=-a*1.41;
+            ledturn();
+            break;
+        case 3:
+            canOmega[0]=a;
+            canOmega[1]=0;
+            canOmega[2]=-a;
+            canOmega[3]=0;
+            ledturn();
+            break;
+        case 4:
+            canOmega[0]=-a*1.41;
+            canOmega[1]=a*1.41;
+            canOmega[2]=a*1.41;
+            canOmega[3]=-a*1.41;
+            ledturn();
+            break;
+        case 5:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=0;
+            }
+            break;
+        case 6:
+            canOmega[0]=a*1.41;
+            canOmega[1]=-a*1.41;
+            canOmega[2]=-a*1.41;
+            canOmega[3]=a*1.41;
+            ledturn();
+            break;
+        case 7:
+            canOmega[0]=0;
+            canOmega[1]=-a;
+            canOmega[2]=0;
+            canOmega[3]=a;
+            ledturn();
+            break;
+        case 8:
+            canOmega[0]=-a*1.41;
+            canOmega[1]=-a*1.41;
+            canOmega[2]=a*1.41;
+            canOmega[3]=a*1.41;
+            ledturn();
+            break;
+        case 9:
+            canOmega[0]=-a;
+            canOmega[1]=0;
+            canOmega[2]=a;
+            canOmega[3]=0;
+            ledturn();
+            break;
+        case 10:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=a;
+            }
+            break;
+        case 11:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=-a;
+            }
+            break;
+        case 12:
+            wait(0.2);
+            if(count==1) {
+                a=0.1;
+                count=0;
+            } else {
+                a=0.05;
+                count=1;
+            }
+            ledturn();
+            break;
+
+    }
+    for(int i=0; i<4; i++) {
+        if(canOmega[i]>0) {
+            canOmega[i]=canOmega[i]*0.8;
+        }
+        motor[i].turn(canOmega[i]);
+    }
+}
+
+//ticker に入れる関数
+Ticker canTicker;
+
+CAN_ticker canx;
+void ticker_CanRead()
+{
+    canx.canmsg_read();
+}
+
+int main()
+{
+    can1.frequency(1000000);
+    printf("start\r\n");
+    motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
+    motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
+    motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
+    motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
+
+    motor[0].setDutyLimit(0.4);
+    motor[1].setDutyLimit(0.4);
+    motor[2].setDutyLimit(0.4);
+    motor[3].setDutyLimit(0.4);
+
+    motor[0].setPDparam( 0.01790, 0.00560);
+    motor[1].setPDparam( 0.01705, 0.00620);
+    motor[2].setPDparam( 0.01790, 0.00620);
+    motor[3].setPDparam( 0.01680, 0.00560);
+
+    int canX=20;//can変数
+    canTicker.attach(&ticker_CanRead,0.05);//can読み込み
+    while(1) {
+        printf("waiting\r\n");
+        canX=canx.get_xCAN();//０で手\動化
+        if(canX==13) {//手動化
+            wait(1);
+            canX=5;
+            while(1) {
+                printf("go\r\n");
+                calOmega_CAN(canX);
+                if(canX==13) {
+                    wait(0.5);
+                    break;
+                }
+                if(canX==14) {
+                    printf("reset\r\n");
+                    NVIC_SystemReset();
+                }
+                canX=canx.get_xCAN();
+            }
+        }
+    }
+}
\ No newline at end of file