Diff: motor_ctl.h

Revision:

25:7764baddb7bc

Parent:

18:0e0edd4f9e4d

Child:

26:2063ee8419cc

--- a/motor_ctl.h	Mon Apr 13 02:43:08 2020 +0000
+++ b/motor_ctl.h	Mon Apr 13 21:32:00 2020 +0000
@@ -1,5 +1,3 @@
-// Move servos in playback mode
-
 #include "mbed.h"
 #include "Servo.h"
 #include "rtos.h"
@@ -14,68 +12,85 @@
 Thread t_r_leg;
 Thread t_l_leg;
 
-DigitalOut led(LED4); 
+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 short move_map[] {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4};
+unsigned short move_map1[] = {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4};
 unsigned short ra_mask = 0x0C0;
 unsigned short la_mask = 0x300;
 unsigned short rl_mask = 0x03C;
 unsigned short ll_mask = 0x007; 
 
-//for testing run through move map
+unsigned short move_map[] = {1,2,4,1,2,4,1,2,4,1,2,4,1,2,4,1,2,4};
 
-void ra_dance(unsigned short move) {    
-    if (move & ra_mask == 0x040){
-        r_arm.write(.50);
-    } else {
-        r_arm.write(.25);
+void ra_dance() {
+    //r_arm = 0;
+    //wait(1);
+    for(int p=0; p<12; ++p) {
+        unsigned short pos = move_map[p];
+        switch(pos){
+            case 1: r_arm.write(1.0);
+            case 2: r_arm.write(0.5);
+            case 4: r_arm.write(1.0);
+        }
+        wait(0.5);
+        r_arm = 0.5;
     }
     
+    r_arm = 1;
+    wait(0.5);
+    r_arm = 0.5;
+    wait(0.5);
+    r_arm = 1; 
+    r_arm.write(.50);
+    r_arm.write()
+
 }
 
-void la_dance(unsigned short move) {    
-     if (move & ra_mask == 0x100){
-        l_arm.write(.50);
-    } else {
-        l_arm.write(.25);
+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; 
+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; 
+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); 
-    }
+void puppet_move(){
+    //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);
+  // }
+  ra_dance();
 }
 
-