Diff: main.cpp

Revision:

0:a1238c4cd105

Child:

1:ea880e226e5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 19 12:43:26 2022 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "PIDco.hpp"
+#include "TARGETco.hpp"
+#include "DUALSHOCKco.hpp"
+#include "sbdbt.hpp"
+#include "BNO055.h"
+
+#define SDA D3
+#define SCL D6
+#define PI 3.1415926535897
+
+Serial pc(USBTX,USBRX);
+
+RotaryInc data_1(PA_14,PA_15,0);
+RotaryInc data_2(PA_12,PC_5,0);
+RotaryInc data_3(PC_0,PC_1,0);
+RotaryInc data_4(PC_2,PC_3,0);
+
+PIDco pid_1;
+PIDco pid_2;
+PIDco pid_3;
+PIDco pid_4;
+
+TARGETco TG;
+BNO055 bno(SDA,SCL);
+DUALSHOCKco DS;
+
+sbdbt sb(PA_0,PA_1);
+
+Timer Time;
+double timer;
+double theta;
+double X,Y;
+
+int main(){
+    Time.start();
+    
+    while(1){
+        timer = Time.read_us();
+        bno.setmode(OPERATION_MODE_IMUPLUS);
+        bno.get_angles();
+        theta = (360 - (double)bno.euler.yaw) / 180 * PI;
+    
+        if(theta > PI){
+            theta = theta - 2 * PI;
+        }
+        
+        DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
+        
+        if(DS.cal_input() == true){
+            TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
+        }
+        else{
+            TG.pass_target(DS.obt_X());
+        }
+        
+        pid_1.pass_val(data_1.get(),TG.obt_target1());
+        pid_2.pass_val(data_2.get(),TG.obt_target2());
+        pid_3.pass_val(data_3.get(),TG.obt_target3());
+        pid_4.pass_val(data_4.get(),TG.obt_target4());
+        
+        pid_1.wheel_ctl(PB_9,PB_8);
+        pid_2.wheel_ctl(PB_14,PB_13);
+        pid_3.wheel_ctl(PB_5,PB_4);
+        pid_4.wheel_ctl(PB_7,PB_6);
+        
+        pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
+        while(Time.read_us() - timer <= 50 * 1000);
+        }
+}
+
+//BNOのピン(PB_3,PB_10)
+        
+        
+        
\ No newline at end of file