Diff: motor_ctl.h

Revision:

21:a279bb16a37b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_ctl.h	Wed Apr 08 21:28:53 2020 +0000
@@ -0,0 +1,88 @@
+// Hello World to sweep a servo through its full range
+
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+
+Servo r_arm(p24);
+Servo l_arm(p23);
+Servo r_leg(p22);
+Servo l_leg(p21);
+
+Thread t_r_arm;
+Thread t_l_arm;
+Thread t_r_leg;
+Thread t_l_leg;
+
+DigitalOut led(LED4); 
+
+//left sensor move whole left side
+//right sensor moves whole right side //not gonna think about this right now. Dicuss ideas with team. 
+//and then the distance can just control the height
+//movement pattern can be random.. right now every other note recorded will be distributed to one side and the rest will go to the other
+
+
+float dists[] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20}; //should I lock this or just make new arrs?
+float r_dists[] = {1,3,5,7,9,11,13,15,17,19};
+float l_dists[] = {2,4,6,8,10,12,14,16,18,20};
+
+
+void ra_dance() {    
+    r_arm = 0; 
+    wait(1);
+    for(float p=0; p<1.0; p += 0.1) {
+        r_arm = p;
+        wait(0.2);
+    }
+    r_arm.write(.50);
+    
+}
+
+void la_dance() {    
+    l_arm = 0; 
+    wait(1);
+    for(float p=0; p<1.0; p += 0.1) {
+        l_arm = p;
+        wait(0.2);
+    }
+    l_arm.write(.50);
+    
+}
+
+void rl_dance() {    
+    r_leg = 0; 
+    wait(1);
+    for(float p=0; p<1.0; p += 0.1) {
+        r_leg = p;
+        wait(0.2);
+    }
+    r_leg.write(.50);
+    
+}
+
+void ll_dance() {    
+    l_leg = 0; 
+    wait(1);
+    for(float p=0; p<1.0; p += 0.1) {
+        l_leg = p;
+        wait(0.2);
+    }
+    l_leg.write(.50);
+    
+}
+
+
+void move_motors(){
+    while(1){
+    t_r_arm.start(ra_dance);
+    wait(1);
+    t_l_arm.start(la_dance);
+    wait(1);
+    t_r_leg.start(rl_dance);
+    wait(1);
+    t_l_leg.start(ll_dance);
+    wait(1);
+   }
+}
+
+