Diff: motor_ctl.h

Revision:

26:2063ee8419cc

Parent:

25:7764baddb7bc

--- a/motor_ctl.h	Mon Apr 13 21:32:00 2020 +0000
+++ b/motor_ctl.h	Tue Apr 14 01:40:26 2020 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "Servo.h"
 #include "rtos.h"
+#include <string>
 
 Servo r_arm(p24);
 Servo l_arm(p23);
@@ -12,85 +13,49 @@
 Thread t_r_leg;
 Thread t_l_leg;
 
-DigitalOut led(LED4);
+#define DUR 200
+
+vector<string> move_map; //initialized in main
+string note; 
 
-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; 
-
-unsigned short move_map[] = {1,2,4,1,2,4,1,2,4,1,2,4,1,2,4,1,2,4};
+void la_dance() {
+    switch(note[0]){
+        case 0: l_arm.write(1.0); break; 
+        case 1: l_arm.write(0.0); break; 
+    }
+    Thread::wait(DUR);
+}
 
 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;
+    switch(note[1]){
+        case '0': r_arm.write(1.0); break; 
+        case '1': r_arm.write(0.0); break; 
     }
-    
-    r_arm = 1;
-    wait(0.5);
-    r_arm = 0.5;
-    wait(0.5);
-    r_arm = 1; 
-    r_arm.write(.50);
-    r_arm.write()
-
+    Thread::wait(DUR);
 }
 
-void la_dance() {
-    l_arm = 0;
-    wait(1);
-    for(float p=0; p<1.0; p += 0.1) {
-        l_arm = p;
-        wait(0.2);
+void ll_dance() {
+    switch(note[2]){
+        case 1: r_leg.write(1.0); break; 
+        case 2: r_leg.write(0.6); break; 
+        case 3: r_leg.write(0.1); break; 
     }
-    l_arm.write(.50);
-
+    Thread::wait(DUR);
 }
 
 void rl_dance() {
-    r_leg = 0;
-    wait(1);
-    for(float p=0; p<1.0; p += 0.1) {
-        r_leg = p;
-        wait(0.2);
+    switch(note[3]){
+        case 1: l_leg.write(1.0); break; 
+        case 2: l_leg.write(0.6); break; 
+        case 3: l_leg.write(0.1); break; 
     }
-    r_leg.write(.50);
-
+    Thread::wait(DUR);
 }
 
-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 puppet_move(){
+    t_r_arm.start(ra_dance);
+    t_l_arm.start(la_dance);
+    t_r_leg.start(rl_dance);
+    t_l_leg.start(ll_dance);
 }
 
-
-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();
-}
-