File content as of revision 16:62976c3c029e:

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