Diff: main.cpp

Revision:

5:e189d55ef292

Parent:

1:ea880e226e5a

--- a/main.cpp	Thu Jan 20 07:27:53 2022 +0000
+++ b/main.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -3,6 +3,7 @@
 #include "PIDco.hpp"
 #include "TARGETco.hpp"
 #include "DUALSHOCKco.hpp"
+#include "Odmetry.hpp"
 #include "sbdbt.hpp"
 #include "BNO055.h"
 
@@ -17,6 +18,11 @@
 RotaryInc data_3(PC_0,PC_1,0);
 RotaryInc data_4(PC_2,PC_3,0);
 
+RotaryInc MW_1(PA_13,PC_4,0);
+RotaryInc MW_2(PC_10,PC_11,0);
+RotaryInc MW_3(PA_7,PA_6,0);
+RotaryInc MW_4(PA_8,PA_9,0);
+
 PIDco pid_1;
 PIDco pid_2;
 PIDco pid_3;
@@ -25,17 +31,17 @@
 TARGETco TG;
 BNO055 bno(SDA,SCL);
 DUALSHOCKco DS;
-
+Odmetry odmetry;
 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);
@@ -47,6 +53,8 @@
             theta = -(2 * PI - theta);
         }
         
+        theta = theta - PI / 4;
+        
         DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
         
         if(DS.cal_input() == true){
@@ -66,7 +74,10 @@
         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());
+        odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
+        odmetry.pass_angle(theta);
+        odmetry.print_Pos();
+        //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);
         }
 }