Diff: motor_ctl_t.h

Revision:

25:7764baddb7bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_ctl_t.h	Mon Apr 13 21:32:00 2020 +0000
@@ -0,0 +1,113 @@
+// 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); 
+
+int i = 0; 
+int j = 0; 
+
+//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 ra_mask = 0x0C0;
+unsigned short la_mask = 0x300;
+unsigned short rl_mask = 0x03C;
+unsigned short ll_mask = 0x007; 
+
+//for testing run through move map
+
+// new class to play a note on Speaker based on PwmOut class
+class Puppet
+{
+public:
+    Puppet(){ //do I need pins if I keep the global var?
+    }
+    void dance(unsigned short[]);
+    void ra_dance(unsigned short);
+    void la_dance(unsigned short);
+    void nextmove();
+private:
+    Timeout moveduration;
+    int movecount;
+    float vol;
+    unsigned short * movementptr;
+};
+//Interrupt Routine to play next note
+void Puppet::nextmove()
+{
+    //_pin = 0.0; //need to set the servo to start up here?
+    r_arm.write(0);
+    l_arm.write(0);
+    movecount++; //setup next note in song
+    if (movementptr[movecount++] == 0.0) movecount = 0;
+    ra_dance(movementptr[movecount]);
+    la_dance(movementptr[movecount]);
+    moveduration.attach(this,&Puppet::nextmove, 0.2);
+
+}
+
+
+void Puppet::ra_dance(unsigned short move) {    
+    /*
+    if (move & ra_mask == 0x040){
+        r_arm.write(1);
+    } else {
+        r_arm.write(.2);
+    }*/
+    if(i > 1) i = 0;
+    r_arm.write(i); 
+    i += 0.1; 
+    
+}
+
+void Puppet::la_dance(unsigned short move) {    
+    /* if (move & ra_mask == 0x100){
+        l_arm.write(1);
+    } else {
+        l_arm.write(.25);
+    }*/
+    if(j > 1) j = 0;
+    l_arm.write(i); 
+    j += 0.1; 
+}
+
+void Puppet::dance(unsigned short movement[]) {
+    movecount = 0;
+    movementptr = movement; 
+    ra_dance(movement[movecount]);
+    la_dance(movement[movecount]);
+    moveduration.attach(this,&Puppet::nextmove, 0.2);
+    // setup timer to interrupt for next note to play
+    
+    //returns after first note starts to play
+}
+
+void puppet_move(){
+    Puppet pupper;
+// Start song and return once playing starts
+    pupper.dance(move_map);
+    // loops forever while song continues to play to end using interrupts
+    while(1) {
+        led = !led;
+        wait(.1);
+    }
+}
+
+
+