File content as of revision 21:a279bb16a37b:

// Hello World to sweep a servo through its full range

#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


float dists[] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20}; //should I lock this or just make new arrs?
float r_dists[] = {1,3,5,7,9,11,13,15,17,19};
float l_dists[] = {2,4,6,8,10,12,14,16,18,20};


void ra_dance() {    
    r_arm = 0; 
    wait(1);
    for(float p=0; p<1.0; p += 0.1) {
        r_arm = p;
        wait(0.2);
    }
    r_arm.write(.50);
    
}

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; 
    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(){
    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);
   }
}