Diff: Control.cpp

Revision:

3:42a8da1837f7

Parent:

2:43e1336c80e4

Child:

4:90b840185921

--- a/Control.cpp	Fri Dec 27 08:16:50 2019 +0000
+++ b/Control.cpp	Sat Dec 28 06:10:55 2019 +0000
@@ -1,17 +1,22 @@
 #include "mbed.h"
 #include "Control.h"
 #include "pinmap.h"
+#include "SBDBT.h"
 void control::setup(){
     sb.setup(0);
 }
 
-void control::OmniControl(){
-    
+void control::run()
+{
+    sb.PS3_JOYSTICK();
+    OmniControl();
+    ModuleControl();
+}
+void control::OmniControl()
+{    
     Vx = sb.JoyStick_dt[0] * -1;
     Vy = sb.JoyStick_dt[1];
     wl = sb.JoyStick_dt[2];
-    /*vx = sb.UP_bit()
-    vy = sb.DOWN_bit()*/
     
     
     pwm[0]=     (Vx - Vy)*bias[0] + wl;
@@ -32,15 +37,43 @@
     mt2.move(pwm[1]/130.0f);
     mt3.move(pwm[2]/130.0f);
     mt4.move(pwm[3]/130.0f);
-    
-    
-    
-    
-    
+    mt5.move(pwm[4]/130.0f);
+    mt6.move(pwm[5]/130.0f);
 }
 
-void control::run(){
- 
-    OmniControl();   
-}  
+
+    int control::CheckButton(int status,int button)
+{
+    if(status == 1){
+        pre_button[button] = 1;
+        if(pre_button[button] == 0){
+            return 1;
+        }
+    }else{
+        pre_button[button] = 0;
+    }
+    return 0;
+}   
+
+void control::ModuleControl()
+{
+    if(CheckButton(sb.PS3_TRIANGLE_bit(),TRIANGLE) && s_time > 100){ 
+        pwm[4] = 0.40f;
+    }else{
+        pwm[4] = 0.0f;
+    }
+    
+    if(CheckButton(sb.PS3_CROSS_bit(),CROSS) && s_time > 100){
+        pwm[4] = 0.40f;
+    }else{
+        pwm[4] = 0.0f;
+    }
+    
+    if(CheckButton(sb.PS3_CIRCLE_bit(),CIRCLE)){
+        pwm[5] = 0.40f;
+    }else{
+        pwm[5] = 0.0f;
+    }
+}
+
     
\ No newline at end of file