Diff: Control.cpp

Revision:

0:ed8bf5304fbb

Child:

1:9c7744271d72

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control.cpp	Sat Dec 28 01:43:29 2019 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "Control.h"
+#include "SBDBT.h"
+#include "pinmap.h"
+
+void control::setup(){
+    for(int i = 0; i <= 4; i++){
+         bias[i]=0.0f;
+    }
+
+}
+
+void control::run(){
+    
+    sb.PS3_JOYSTICK();
+    OmniControl();
+
+}
+
+void control::OmniControl()
+{
+    Vx = sb.JoyStick_dt[0] * -1;
+    Vy = sb.JoyStick_dt[1];
+    wl = sb.JoyStick_dt[2];
+    
+    pwm[0]=(Vx -Vy) * bias[0] + wl;
+    pwm[1]=(-1)*(Vx -Vy) * bias[1] + wl;
+    pwm[2]=(-1)*(Vx -Vy) * bias[2] + wl;
+    pwm[3]=(Vx -Vy) * bias[3] + wl;
+    
+    for(int i = 0; i < 4; i++){
+        if(pwm[i] > 127){
+            pwm[i] = 127;
+        }else if(pwm[i] < -127){
+            pwm[i] = -127;
+        }
+}
+
+    mt1.move(pwm[0]/130.0f);
+    mt2.move(pwm[1]/130.0f);
+    mt3.move(pwm[2]/130.0f);
+    mt4.move(pwm[3]/130.0f);
+}
+
+void control::SolenoidControl()
+{   if(sb.PS3_CIRCLE_bit() == 1){
+        if(pre_button sircle[0]==0){
+            sol.write(0,1);
+    }else{
+            pre_button sircle==1;
+            sol.write(0,0);
+            
+        }
+    } 
+    if(sb.PS3_Triangle_bit() == 1){
+        if(pre_button triangle[0]==0){
+            sol.write(1,1);
+    }else{
+            pre_button triangle==1;
+            sol.write(1,0);
+            
+        }
+    } 
+    if(sb.PS3_square_bit() == 1){
+        if(pre_button square[0]==0){
+            sol.write(2,1);
+    }else{
+            pre_button square==1;
+            sol.write(2,0);
+            
+        }
+    }
+    if(sb.PS3_cross_bit() == 1){
+        if(pre_button cross[0]==0){
+            sol.write(3,1);
+    }else{
+            pre_button cross==1;
+            sol.write(3,0);
+            
+        }
+    }    
+}
+
+    
\ No newline at end of file