2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

Diff: main.cpp

Revision:
2:612efbe94f42
Parent:
1:ea880e226e5a
Child:
4:867c2b5cf81a
--- a/main.cpp	Thu Jan 20 07:27:53 2022 +0000
+++ b/main.cpp	Fri Jan 21 08:57:07 2022 +0000
@@ -5,12 +5,14 @@
 #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
 
-Serial pc(USBTX,USBRX);
+ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);
 
 RotaryInc data_1(PA_14,PA_15,0);
 RotaryInc data_2(PA_12,PC_5,0);
@@ -26,7 +28,12 @@
 BNO055 bno(SDA,SCL);
 DUALSHOCKco DS;
 
-sbdbt sb(PA_0,PA_1);
+//sbdbt sb(PA_0,PA_1);
+
+DigitalOut led1(PA_10);
+DigitalOut led2(PB_15);
+DigitalOut led3(PB_2);
+DigitalOut led4(PC_6);
 
 Timer Time;
 double timer;
@@ -35,6 +42,13 @@
 
 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();
@@ -47,7 +61,7 @@
             theta = -(2 * PI - theta);
         }
         
-        DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
+        DS.pass_val(x_component,y_component,r2_num,l2_num);
         
         if(DS.cal_input() == true){
             TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
@@ -66,7 +80,28 @@
         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());
+        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());
         while(Time.read_us() - timer <= 50 * 1000);
         }
 }