Diff: motor_ctl.h

Revision:

16:62976c3c029e

Child:

18:0e0edd4f9e4d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_ctl.h	Mon Apr 13 02:16:45 2020 +0000
@@ -0,0 +1,81 @@
+// Move servos in playback mode
+
+#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
+
+unsigned char move_map {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4};
+unsigned char ra_mask = 0x0C0;
+unsigned char la_mask = 0x300;
+unsigned char rl_mask = 0x03C;
+unsigned char ll_mask = 0x007; 
+
+//for testing run through move map
+
+void ra_dance(unsigned char move) {    
+    if (move & ra_mask == 0x040){
+        r_arm.write(.50);
+    } else {
+        r_arm.write(.25);
+    }
+    
+}
+
+void la_dance() {    
+     if (move & ra_mask == 0x100){
+        l_arm.write(.50);
+    } else {
+        l_arm.write(.25);
+    }
+    
+}
+
+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(){
+    for (int i = 0; i < 12; ++i){
+        ra_dance(move_map[i]);
+        la_dance(move_map[i]);
+        wait(0.2); 
+    }
+}
+
+