Diff: main.cpp

Revision:

0:4f39632d7a42

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jan 17 02:40:16 2022 +0000
@@ -0,0 +1,88 @@
+//------INCLUDE-------
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "PIDcontrol.hpp"
+#include "sbdbt.hpp"
+
+//------CLASS_DEFINE------
+Serial pc(USBTX,USBRX);
+
+RotaryInc Ror1(PA_6,PA_7,0);
+RotaryInc Ror2(PC_10,PC_11,0);
+RotaryInc Ror3(PA_8,PA_9,0);
+RotaryInc Ror4(PA_14,PA_15,0);
+
+PIDcontrol pid_1;
+PIDcontrol pid_2;
+PIDcontrol pid_3;
+PIDcontrol pid_4;
+
+sbdbt sb(PA_0,PA_1);
+
+//------VARIABLE-------
+Timer one;
+double val_pulse[4];
+double val_spd[4];
+double val_target[4];
+double timer;
+const double PI=3.1415926535897;
+double theta,angle=45;
+double Xvalue,Yvalue,Xvelocity,Yvelocity;
+//------FUNCTION-------
+void get_pulse(void);
+void angular(void);
+
+int main(){
+    one.start();
+    while(1){
+        timer = one.read_us();
+        get_pulse();
+        angular();
+        //pid_a.motor_control(targetの値,pulseの値,PinName pin_a,PinName pin_b);
+        pid_1.motor_control(val_target[0],val_pulse[0],PB_14,PB_13);
+        pid_2.motor_control(val_target[1],val_pulse[1],PC_9,PC_8);
+        pid_3.motor_control(val_target[2],val_pulse[2],PB_5,PB_4);
+        pid_4.motor_control(val_target[3],val_pulse[3],PA_11,PB_1);
+        
+        val_spd[0]=pid_1.get_spd();
+        val_spd[1]=pid_2.get_spd();
+        val_spd[2]=pid_3.get_spd();
+        val_spd[3]=pid_4.get_spd();
+        
+        pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",val_target[0],val_target[1],val_target[2],val_target[3],val_spd[0],val_spd[1],val_spd[2],val_spd[3]);
+        while(one.read_us() - timer <= 50 * 1000); //0.05s周期
+        }
+}
+
+//-------FUNCTION_DEFINE------
+void get_pulse(){
+    val_pulse[0]=Ror1.get();
+    val_pulse[1]=Ror2.get();
+    val_pulse[2]=Ror3.get();
+    val_pulse[3]=Ror4.get();
+}
+void angular(){
+    theta=(angle/180.0)*PI;
+    if(sb.rsx()!=0||sb.rsx()!=0){
+    Xvalue=(sb.rsx()-64)*500/64.0;
+    Yvalue=(sb.rsy()-64)*500/64.0;
+    Xvelocity=Xvalue*cos(theta)-Yvalue*sin(theta);
+    Yvelocity=Xvalue*sin(theta)+Yvalue*cos(theta);
+    val_target[0]=-Xvelocity;
+    val_target[1]=-Yvelocity;
+    val_target[2]=Xvelocity;
+    val_target[3]=Yvelocity;
+    }
+    if(sb.l2()!=0){
+    Yvalue=sb.l2()*500;
+    for(int i=0;i<=3;i++){
+        val_target[i]=-Yvalue;
+     }
+    }
+    if(sb.r2()!=0){
+        Yvalue=sb.r2()*500;
+        for(int i=0;i<=3;i++){
+        val_target[i]=Yvalue;
+        }
+    }
+}