File content as of revision 26:2063ee8419cc:

// 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);
    }
}