Carter Montgomery / Mbed 2 deprecated VoodooBoi9000

ECE3872 HW/SW Project Code

Dependencies:   mbed Servo mbed-rtos 4DGL-uLCD-SE PinDetect X_NUCLEO_53L0A1

motor_ctl.h@21:a279bb16a37b, 2020-04-08 (annotated)

Committer:
trmontgomery
Date:
Wed Apr 08 21:28:53 2020 +0000
Revision:
21:a279bb16a37b
stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
trmontgomery 21:a279bb16a37b 1 // Hello World to sweep a servo through its full range
trmontgomery 21:a279bb16a37b 2
trmontgomery 21:a279bb16a37b 3 #include "mbed.h"
trmontgomery 21:a279bb16a37b 4 #include "Servo.h"
trmontgomery 21:a279bb16a37b 5 #include "rtos.h"
trmontgomery 21:a279bb16a37b 6
trmontgomery 21:a279bb16a37b 7 Servo r_arm(p24);
trmontgomery 21:a279bb16a37b 8 Servo l_arm(p23);
trmontgomery 21:a279bb16a37b 9 Servo r_leg(p22);
trmontgomery 21:a279bb16a37b 10 Servo l_leg(p21);
trmontgomery 21:a279bb16a37b 11
trmontgomery 21:a279bb16a37b 12 Thread t_r_arm;
trmontgomery 21:a279bb16a37b 13 Thread t_l_arm;
trmontgomery 21:a279bb16a37b 14 Thread t_r_leg;
trmontgomery 21:a279bb16a37b 15 Thread t_l_leg;
trmontgomery 21:a279bb16a37b 16
trmontgomery 21:a279bb16a37b 17 DigitalOut led(LED4);
trmontgomery 21:a279bb16a37b 18
trmontgomery 21:a279bb16a37b 19 //left sensor move whole left side
trmontgomery 21:a279bb16a37b 20 //right sensor moves whole right side //not gonna think about this right now. Dicuss ideas with team.
trmontgomery 21:a279bb16a37b 21 //and then the distance can just control the height
trmontgomery 21:a279bb16a37b 22 //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 21:a279bb16a37b 23
trmontgomery 21:a279bb16a37b 24
trmontgomery 21:a279bb16a37b 25 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?
trmontgomery 21:a279bb16a37b 26 float r_dists[] = {1,3,5,7,9,11,13,15,17,19};
trmontgomery 21:a279bb16a37b 27 float l_dists[] = {2,4,6,8,10,12,14,16,18,20};
trmontgomery 21:a279bb16a37b 28
trmontgomery 21:a279bb16a37b 29
trmontgomery 21:a279bb16a37b 30 void ra_dance() {
trmontgomery 21:a279bb16a37b 31 r_arm = 0;
trmontgomery 21:a279bb16a37b 32 wait(1);
trmontgomery 21:a279bb16a37b 33 for(float p=0; p<1.0; p += 0.1) {
trmontgomery 21:a279bb16a37b 34 r_arm = p;
trmontgomery 21:a279bb16a37b 35 wait(0.2);
trmontgomery 21:a279bb16a37b 36 }
trmontgomery 21:a279bb16a37b 37 r_arm.write(.50);
trmontgomery 21:a279bb16a37b 38
trmontgomery 21:a279bb16a37b 39 }
trmontgomery 21:a279bb16a37b 40
trmontgomery 21:a279bb16a37b 41 void la_dance() {
trmontgomery 21:a279bb16a37b 42 l_arm = 0;
trmontgomery 21:a279bb16a37b 43 wait(1);
trmontgomery 21:a279bb16a37b 44 for(float p=0; p<1.0; p += 0.1) {
trmontgomery 21:a279bb16a37b 45 l_arm = p;
trmontgomery 21:a279bb16a37b 46 wait(0.2);
trmontgomery 21:a279bb16a37b 47 }
trmontgomery 21:a279bb16a37b 48 l_arm.write(.50);
trmontgomery 21:a279bb16a37b 49
trmontgomery 21:a279bb16a37b 50 }
trmontgomery 21:a279bb16a37b 51
trmontgomery 21:a279bb16a37b 52 void rl_dance() {
trmontgomery 21:a279bb16a37b 53 r_leg = 0;
trmontgomery 21:a279bb16a37b 54 wait(1);
trmontgomery 21:a279bb16a37b 55 for(float p=0; p<1.0; p += 0.1) {
trmontgomery 21:a279bb16a37b 56 r_leg = p;
trmontgomery 21:a279bb16a37b 57 wait(0.2);
trmontgomery 21:a279bb16a37b 58 }
trmontgomery 21:a279bb16a37b 59 r_leg.write(.50);
trmontgomery 21:a279bb16a37b 60
trmontgomery 21:a279bb16a37b 61 }
trmontgomery 21:a279bb16a37b 62
trmontgomery 21:a279bb16a37b 63 void ll_dance() {
trmontgomery 21:a279bb16a37b 64 l_leg = 0;
trmontgomery 21:a279bb16a37b 65 wait(1);
trmontgomery 21:a279bb16a37b 66 for(float p=0; p<1.0; p += 0.1) {
trmontgomery 21:a279bb16a37b 67 l_leg = p;
trmontgomery 21:a279bb16a37b 68 wait(0.2);
trmontgomery 21:a279bb16a37b 69 }
trmontgomery 21:a279bb16a37b 70 l_leg.write(.50);
trmontgomery 21:a279bb16a37b 71
trmontgomery 21:a279bb16a37b 72 }
trmontgomery 21:a279bb16a37b 73
trmontgomery 21:a279bb16a37b 74
trmontgomery 21:a279bb16a37b 75 void move_motors(){
trmontgomery 21:a279bb16a37b 76 while(1){
trmontgomery 21:a279bb16a37b 77 t_r_arm.start(ra_dance);
trmontgomery 21:a279bb16a37b 78 wait(1);
trmontgomery 21:a279bb16a37b 79 t_l_arm.start(la_dance);
trmontgomery 21:a279bb16a37b 80 wait(1);
trmontgomery 21:a279bb16a37b 81 t_r_leg.start(rl_dance);
trmontgomery 21:a279bb16a37b 82 wait(1);
trmontgomery 21:a279bb16a37b 83 t_l_leg.start(ll_dance);
trmontgomery 21:a279bb16a37b 84 wait(1);
trmontgomery 21:a279bb16a37b 85 }
trmontgomery 21:a279bb16a37b 86 }
trmontgomery 21:a279bb16a37b 87
trmontgomery 21:a279bb16a37b 88