motor_ctl_t.h

00001 // Move servos in playback mode
00002 
00003 #include "mbed.h"
00004 #include "Servo.h"
00005 #include "rtos.h"
00006 
00007 Servo r_arm(p24);
00008 Servo l_arm(p23);
00009 Servo r_leg(p22);
00010 Servo l_leg(p21);
00011 
00012 Thread t_r_arm;
00013 Thread t_l_arm;
00014 Thread t_r_leg;
00015 Thread t_l_leg;
00016 
00017 DigitalOut led(LED4); 
00018 
00019 int i = 0; 
00020 int j = 0; 
00021 
00022 //left sensor move whole left side
00023 //right sensor moves whole right side //not gonna think about this right now. Dicuss ideas with team. 
00024 //and then the distance can just control the height
00025 //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
00026 
00027 unsigned short move_map[] {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4};
00028 unsigned short ra_mask = 0x0C0;
00029 unsigned short la_mask = 0x300;
00030 unsigned short rl_mask = 0x03C;
00031 unsigned short ll_mask = 0x007; 
00032 
00033 //for testing run through move map
00034 
00035 // new class to play a note on Speaker based on PwmOut class
00036 class Puppet
00037 {
00038 public:
00039     Puppet(){ //do I need pins if I keep the global var?
00040     }
00041     void dance(unsigned short[]);
00042     void ra_dance(unsigned short);
00043     void la_dance(unsigned short);
00044     void nextmove();
00045 private:
00046     Timeout moveduration;
00047     int movecount;
00048     float vol;
00049     unsigned short * movementptr;
00050 };
00051 //Interrupt Routine to play next note
00052 void Puppet::nextmove()
00053 {
00054     //_pin = 0.0; //need to set the servo to start up here?
00055     r_arm.write(0);
00056     l_arm.write(0);
00057     movecount++; //setup next note in song
00058     if (movementptr[movecount++] == 0.0) movecount = 0;
00059     ra_dance(movementptr[movecount]);
00060     la_dance(movementptr[movecount]);
00061     moveduration.attach(this,&Puppet::nextmove, 0.2);
00062 
00063 }
00064 
00065 
00066 void Puppet::ra_dance(unsigned short move) {    
00067     /*
00068     if (move & ra_mask == 0x040){
00069         r_arm.write(1);
00070     } else {
00071         r_arm.write(.2);
00072     }*/
00073     if(i > 1) i = 0;
00074     r_arm.write(i); 
00075     i += 0.1; 
00076     
00077 }
00078 
00079 void Puppet::la_dance(unsigned short move) {    
00080     /* if (move & ra_mask == 0x100){
00081         l_arm.write(1);
00082     } else {
00083         l_arm.write(.25);
00084     }*/
00085     if(j > 1) j = 0;
00086     l_arm.write(i); 
00087     j += 0.1; 
00088 }
00089 
00090 void Puppet::dance(unsigned short movement[]) {
00091     movecount = 0;
00092     movementptr = movement; 
00093     ra_dance(movement[movecount]);
00094     la_dance(movement[movecount]);
00095     moveduration.attach(this,&Puppet::nextmove, 0.2);
00096     // setup timer to interrupt for next note to play
00097     
00098     //returns after first note starts to play
00099 }
00100 
00101 void puppet_move(){
00102     Puppet pupper;
00103 // Start song and return once playing starts
00104     pupper.dance(move_map);
00105     // loops forever while song continues to play to end using interrupts
00106     while(1) {
00107         led = !led;
00108         wait(.1);
00109     }
00110 }
00111 
00112 
00113