ECE3872 HW/SW Project Code
Dependencies: mbed Servo mbed-rtos 4DGL-uLCD-SE PinDetect X_NUCLEO_53L0A1
motor_ctl_t.h@25:7764baddb7bc, 2020-04-13 (annotated)
- Committer:
- trmontgomery
- Date:
- Mon Apr 13 21:32:00 2020 +0000
- Revision:
- 25:7764baddb7bc
trying to make a servo move back and forth. tried to use SongPlayer class to control servos.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
trmontgomery | 25:7764baddb7bc | 1 | // Move servos in playback mode |
trmontgomery | 25:7764baddb7bc | 2 | |
trmontgomery | 25:7764baddb7bc | 3 | #include "mbed.h" |
trmontgomery | 25:7764baddb7bc | 4 | #include "Servo.h" |
trmontgomery | 25:7764baddb7bc | 5 | #include "rtos.h" |
trmontgomery | 25:7764baddb7bc | 6 | |
trmontgomery | 25:7764baddb7bc | 7 | Servo r_arm(p24); |
trmontgomery | 25:7764baddb7bc | 8 | Servo l_arm(p23); |
trmontgomery | 25:7764baddb7bc | 9 | Servo r_leg(p22); |
trmontgomery | 25:7764baddb7bc | 10 | Servo l_leg(p21); |
trmontgomery | 25:7764baddb7bc | 11 | |
trmontgomery | 25:7764baddb7bc | 12 | Thread t_r_arm; |
trmontgomery | 25:7764baddb7bc | 13 | Thread t_l_arm; |
trmontgomery | 25:7764baddb7bc | 14 | Thread t_r_leg; |
trmontgomery | 25:7764baddb7bc | 15 | Thread t_l_leg; |
trmontgomery | 25:7764baddb7bc | 16 | |
trmontgomery | 25:7764baddb7bc | 17 | DigitalOut led(LED4); |
trmontgomery | 25:7764baddb7bc | 18 | |
trmontgomery | 25:7764baddb7bc | 19 | int i = 0; |
trmontgomery | 25:7764baddb7bc | 20 | int j = 0; |
trmontgomery | 25:7764baddb7bc | 21 | |
trmontgomery | 25:7764baddb7bc | 22 | //left sensor move whole left side |
trmontgomery | 25:7764baddb7bc | 23 | //right sensor moves whole right side //not gonna think about this right now. Dicuss ideas with team. |
trmontgomery | 25:7764baddb7bc | 24 | //and then the distance can just control the height |
trmontgomery | 25:7764baddb7bc | 25 | //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 |
trmontgomery | 25:7764baddb7bc | 26 | |
trmontgomery | 25:7764baddb7bc | 27 | unsigned short move_map[] {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4}; |
trmontgomery | 25:7764baddb7bc | 28 | unsigned short ra_mask = 0x0C0; |
trmontgomery | 25:7764baddb7bc | 29 | unsigned short la_mask = 0x300; |
trmontgomery | 25:7764baddb7bc | 30 | unsigned short rl_mask = 0x03C; |
trmontgomery | 25:7764baddb7bc | 31 | unsigned short ll_mask = 0x007; |
trmontgomery | 25:7764baddb7bc | 32 | |
trmontgomery | 25:7764baddb7bc | 33 | //for testing run through move map |
trmontgomery | 25:7764baddb7bc | 34 | |
trmontgomery | 25:7764baddb7bc | 35 | // new class to play a note on Speaker based on PwmOut class |
trmontgomery | 25:7764baddb7bc | 36 | class Puppet |
trmontgomery | 25:7764baddb7bc | 37 | { |
trmontgomery | 25:7764baddb7bc | 38 | public: |
trmontgomery | 25:7764baddb7bc | 39 | Puppet(){ //do I need pins if I keep the global var? |
trmontgomery | 25:7764baddb7bc | 40 | } |
trmontgomery | 25:7764baddb7bc | 41 | void dance(unsigned short[]); |
trmontgomery | 25:7764baddb7bc | 42 | void ra_dance(unsigned short); |
trmontgomery | 25:7764baddb7bc | 43 | void la_dance(unsigned short); |
trmontgomery | 25:7764baddb7bc | 44 | void nextmove(); |
trmontgomery | 25:7764baddb7bc | 45 | private: |
trmontgomery | 25:7764baddb7bc | 46 | Timeout moveduration; |
trmontgomery | 25:7764baddb7bc | 47 | int movecount; |
trmontgomery | 25:7764baddb7bc | 48 | float vol; |
trmontgomery | 25:7764baddb7bc | 49 | unsigned short * movementptr; |
trmontgomery | 25:7764baddb7bc | 50 | }; |
trmontgomery | 25:7764baddb7bc | 51 | //Interrupt Routine to play next note |
trmontgomery | 25:7764baddb7bc | 52 | void Puppet::nextmove() |
trmontgomery | 25:7764baddb7bc | 53 | { |
trmontgomery | 25:7764baddb7bc | 54 | //_pin = 0.0; //need to set the servo to start up here? |
trmontgomery | 25:7764baddb7bc | 55 | r_arm.write(0); |
trmontgomery | 25:7764baddb7bc | 56 | l_arm.write(0); |
trmontgomery | 25:7764baddb7bc | 57 | movecount++; //setup next note in song |
trmontgomery | 25:7764baddb7bc | 58 | if (movementptr[movecount++] == 0.0) movecount = 0; |
trmontgomery | 25:7764baddb7bc | 59 | ra_dance(movementptr[movecount]); |
trmontgomery | 25:7764baddb7bc | 60 | la_dance(movementptr[movecount]); |
trmontgomery | 25:7764baddb7bc | 61 | moveduration.attach(this,&Puppet::nextmove, 0.2); |
trmontgomery | 25:7764baddb7bc | 62 | |
trmontgomery | 25:7764baddb7bc | 63 | } |
trmontgomery | 25:7764baddb7bc | 64 | |
trmontgomery | 25:7764baddb7bc | 65 | |
trmontgomery | 25:7764baddb7bc | 66 | void Puppet::ra_dance(unsigned short move) { |
trmontgomery | 25:7764baddb7bc | 67 | /* |
trmontgomery | 25:7764baddb7bc | 68 | if (move & ra_mask == 0x040){ |
trmontgomery | 25:7764baddb7bc | 69 | r_arm.write(1); |
trmontgomery | 25:7764baddb7bc | 70 | } else { |
trmontgomery | 25:7764baddb7bc | 71 | r_arm.write(.2); |
trmontgomery | 25:7764baddb7bc | 72 | }*/ |
trmontgomery | 25:7764baddb7bc | 73 | if(i > 1) i = 0; |
trmontgomery | 25:7764baddb7bc | 74 | r_arm.write(i); |
trmontgomery | 25:7764baddb7bc | 75 | i += 0.1; |
trmontgomery | 25:7764baddb7bc | 76 | |
trmontgomery | 25:7764baddb7bc | 77 | } |
trmontgomery | 25:7764baddb7bc | 78 | |
trmontgomery | 25:7764baddb7bc | 79 | void Puppet::la_dance(unsigned short move) { |
trmontgomery | 25:7764baddb7bc | 80 | /* if (move & ra_mask == 0x100){ |
trmontgomery | 25:7764baddb7bc | 81 | l_arm.write(1); |
trmontgomery | 25:7764baddb7bc | 82 | } else { |
trmontgomery | 25:7764baddb7bc | 83 | l_arm.write(.25); |
trmontgomery | 25:7764baddb7bc | 84 | }*/ |
trmontgomery | 25:7764baddb7bc | 85 | if(j > 1) j = 0; |
trmontgomery | 25:7764baddb7bc | 86 | l_arm.write(i); |
trmontgomery | 25:7764baddb7bc | 87 | j += 0.1; |
trmontgomery | 25:7764baddb7bc | 88 | } |
trmontgomery | 25:7764baddb7bc | 89 | |
trmontgomery | 25:7764baddb7bc | 90 | void Puppet::dance(unsigned short movement[]) { |
trmontgomery | 25:7764baddb7bc | 91 | movecount = 0; |
trmontgomery | 25:7764baddb7bc | 92 | movementptr = movement; |
trmontgomery | 25:7764baddb7bc | 93 | ra_dance(movement[movecount]); |
trmontgomery | 25:7764baddb7bc | 94 | la_dance(movement[movecount]); |
trmontgomery | 25:7764baddb7bc | 95 | moveduration.attach(this,&Puppet::nextmove, 0.2); |
trmontgomery | 25:7764baddb7bc | 96 | // setup timer to interrupt for next note to play |
trmontgomery | 25:7764baddb7bc | 97 | |
trmontgomery | 25:7764baddb7bc | 98 | //returns after first note starts to play |
trmontgomery | 25:7764baddb7bc | 99 | } |
trmontgomery | 25:7764baddb7bc | 100 | |
trmontgomery | 25:7764baddb7bc | 101 | void puppet_move(){ |
trmontgomery | 25:7764baddb7bc | 102 | Puppet pupper; |
trmontgomery | 25:7764baddb7bc | 103 | // Start song and return once playing starts |
trmontgomery | 25:7764baddb7bc | 104 | pupper.dance(move_map); |
trmontgomery | 25:7764baddb7bc | 105 | // loops forever while song continues to play to end using interrupts |
trmontgomery | 25:7764baddb7bc | 106 | while(1) { |
trmontgomery | 25:7764baddb7bc | 107 | led = !led; |
trmontgomery | 25:7764baddb7bc | 108 | wait(.1); |
trmontgomery | 25:7764baddb7bc | 109 | } |
trmontgomery | 25:7764baddb7bc | 110 | } |
trmontgomery | 25:7764baddb7bc | 111 | |
trmontgomery | 25:7764baddb7bc | 112 | |
trmontgomery | 25:7764baddb7bc | 113 |