Diff: mecanum_4.cpp

Revision:

0:b7d3adaffc0a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mecanum_4.cpp	Mon May 07 06:57:22 2018 +0000
@@ -0,0 +1,67 @@
+#include "mecanum_4.h"
+
+//four motors
+Motor_3 front_left(PB_3,PB_4,PB_1);
+Motor_3 front_right(PA_12,PA_15,PB_0);
+Motor_3 back_left(PB_13,PB_12,PA_6);
+Motor_3 back_right(PB_15,PB_14,PA_7); 
+
+void mecanum_4::mv_x(float speed)
+{
+    front_left.mv(speed);   front_right.mv(speed);
+    back_left.mv(speed);    back_right.mv(speed);
+}
+void mecanum_4::mv_y(float speed)
+{
+    float speed1=-speed; 
+    front_left.mv(speed1);   front_right.mv(speed);
+    back_left.mv(speed);    back_right.mv(speed1);
+}
+void mecanum_4::rotate(float speed)
+{
+    float speed1=-speed;
+    front_left.mv(speed1);   front_right.mv(speed);
+    back_left.mv(speed1);    back_right.mv(speed);
+}
+void mecanum_4::r_ob(float speed)
+{
+    front_left.mv(speed);   front_right.mv(0);
+    back_left.mv(0);    back_right.mv(speed);        
+}
+void mecanum_4::i_ob(float speed)
+{
+    front_left.mv(0);   front_right.mv(speed);
+    back_left.mv(speed);    back_right.mv(0);        
+}
+void mecanum_4::any_degree(float degree,float time)
+{
+    if(degree>=0&&degree<=90)
+    {
+            float degree1=(-degree)/90*200+100;
+            front_left.mv(100);   front_right.mv(degree1);
+            back_left.mv(degree1);    back_right.mv(100);     
+    }   
+    else if(degree>=90&&degree<=180)
+    {
+            float degree1=(-degree)/90*200+300;
+            front_left.mv(degree1);   front_right.mv(-100);
+            back_left.mv(-100);    back_right.mv(degree1);  
+    }
+    else if(degree>=180&&degree<=270)
+    {
+            float degree1=degree/90*200-500;
+            front_left.mv(-100);   front_right.mv(degree1);
+            back_left.mv(degree1);    back_right.mv(-100);  
+    }
+    else if(degree>=270&&degree<=360)
+    {
+            float degree1=degree/90*200-700;
+            front_left.mv(degree1);   front_right.mv(100);
+            back_left.mv(100);    back_right.mv(degree1);  
+    }
+    wait(time);
+    front_left.stop();
+    front_right.stop();
+    back_left.stop();
+    back_right.stop();
+}
\ No newline at end of file