Diff: main.cpp

Revision:

4:920acf362fb6

Parent:

2:612efbe94f42

--- a/main.cpp	Fri Jan 21 09:13:50 2022 +0000
+++ b/main.cpp	Fri Jan 21 09:15:33 2022 +0000
@@ -5,50 +5,36 @@
 #include "DUALSHOCKco.hpp"
 #include "sbdbt.hpp"
 #include "BNO055.h"
-#include "scrp_slave.hpp"
-#include "function.hpp"
-
+ 
 #define SDA D3
 #define SCL D6
 #define PI 3.1415926535897
-
-ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);
-
+ 
+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);
-
-DigitalOut led1(PA_10);
-DigitalOut led2(PB_15);
-DigitalOut led3(PB_2);
-DigitalOut led4(PC_6);
-
+ 
+sbdbt sb(PA_0,PA_1);
+ 
 Timer Time;
 double timer;
 double theta;
 double X,Y;
-
+ 
 int main(){
     Time.start();
-    TG.pass_val(0,0,0);
-    scrp.addCMD(1,getLstick_x);
-    scrp.addCMD(2,getLstick_y);
-    scrp.addCMD(3,getL2);
-    scrp.addCMD(4,getR2);
-    scrp.addCMD(5,change_mode);
-
     
     while(1){
         timer = Time.read_us();
@@ -61,7 +47,7 @@
             theta = -(2 * PI - theta);
         }
         
-        DS.pass_val(x_component,y_component,r2_num,l2_num);
+        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);
@@ -80,33 +66,9 @@
         pid_3.wheel_ctl(PB_5,PB_4);
         pid_4.wheel_ctl(PB_7,PB_6);
         
-        if(TG.obt_target1() != 0){
-            led1 = 1;
-        }else{
-            led1 = 0;
-        }
-        if(TG.obt_target2() != 0){
-            led2 = 1;
-        }else{
-            led2 = 0;
-        }
-        if(TG.obt_target3() != 0){
-            led3 = 1;
-        }else{
-            led3 = 0;
-        }
-        if(TG.obt_target4() != 0){
-            led4 = 1;
-        }else{
-            led4 = 0;
-        }
-        
-        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());
+        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
+ 
+//BNOのピン(PB_3,PB_10)
\ No newline at end of file