ECE3872 HW/SW Project Code
Dependencies:
mbed
Servo
mbed-rtos
4DGL-uLCD-SE
PinDetect
X_NUCLEO_53L0A1
motor_ctl.h@18:0e0edd4f9e4d, 2020-04-13 (annotated)
- Committer:
- trmontgomery
- Date:
- Mon Apr 13 02:43:08 2020 +0000
- Revision:
- 18:0e0edd4f9e4d
- Parent:
-
16:62976c3c029e
- Child:
- 25:7764baddb7bc
all files successfully merged
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|
| trmontgomery |
16:62976c3c029e
|
1
|
// Move servos in playback mode
|
| trmontgomery |
16:62976c3c029e
|
2
|
|
| trmontgomery |
16:62976c3c029e
|
3
|
#include "mbed.h"
|
| trmontgomery |
16:62976c3c029e
|
4
|
#include "Servo.h"
|
| trmontgomery |
18:0e0edd4f9e4d
|
5
|
#include "rtos.h"
|
| trmontgomery |
16:62976c3c029e
|
6
|
|
| trmontgomery |
16:62976c3c029e
|
7
|
Servo r_arm(p24);
|
| trmontgomery |
16:62976c3c029e
|
8
|
Servo l_arm(p23);
|
| trmontgomery |
16:62976c3c029e
|
9
|
Servo r_leg(p22);
|
| trmontgomery |
16:62976c3c029e
|
10
|
Servo l_leg(p21);
|
| trmontgomery |
16:62976c3c029e
|
11
|
|
| trmontgomery |
16:62976c3c029e
|
12
|
Thread t_r_arm;
|
| trmontgomery |
16:62976c3c029e
|
13
|
Thread t_l_arm;
|
| trmontgomery |
16:62976c3c029e
|
14
|
Thread t_r_leg;
|
| trmontgomery |
16:62976c3c029e
|
15
|
Thread t_l_leg;
|
| trmontgomery |
16:62976c3c029e
|
16
|
|
| trmontgomery |
16:62976c3c029e
|
17
|
DigitalOut led(LED4);
|
| trmontgomery |
16:62976c3c029e
|
18
|
|
| trmontgomery |
16:62976c3c029e
|
19
|
//left sensor move whole left side
|
| trmontgomery |
16:62976c3c029e
|
20
|
//right sensor moves whole right side //not gonna think about this right now. Dicuss ideas with team.
|
| trmontgomery |
16:62976c3c029e
|
21
|
//and then the distance can just control the height
|
| trmontgomery |
16:62976c3c029e
|
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 |
16:62976c3c029e
|
23
|
|
| trmontgomery |
18:0e0edd4f9e4d
|
24
|
unsigned short move_map[] {0x149, 0x14A, 0x14C, 0x151, 0x152, 0x154, 0x291, 0x292, 0x294, 0x2A1, 0x2A2, 0x2A4};
|
| trmontgomery |
18:0e0edd4f9e4d
|
25
|
unsigned short ra_mask = 0x0C0;
|
| trmontgomery |
18:0e0edd4f9e4d
|
26
|
unsigned short la_mask = 0x300;
|
| trmontgomery |
18:0e0edd4f9e4d
|
27
|
unsigned short rl_mask = 0x03C;
|
| trmontgomery |
18:0e0edd4f9e4d
|
28
|
unsigned short ll_mask = 0x007;
|
| trmontgomery |
16:62976c3c029e
|
29
|
|
| trmontgomery |
16:62976c3c029e
|
30
|
//for testing run through move map
|
| trmontgomery |
16:62976c3c029e
|
31
|
|
| trmontgomery |
18:0e0edd4f9e4d
|
32
|
void ra_dance(unsigned short move) {
|
| trmontgomery |
16:62976c3c029e
|
33
|
if (move & ra_mask == 0x040){
|
| trmontgomery |
16:62976c3c029e
|
34
|
r_arm.write(.50);
|
| trmontgomery |
16:62976c3c029e
|
35
|
} else {
|
| trmontgomery |
16:62976c3c029e
|
36
|
r_arm.write(.25);
|
| trmontgomery |
16:62976c3c029e
|
37
|
}
|
| trmontgomery |
16:62976c3c029e
|
38
|
|
| trmontgomery |
16:62976c3c029e
|
39
|
}
|
| trmontgomery |
16:62976c3c029e
|
40
|
|
| trmontgomery |
18:0e0edd4f9e4d
|
41
|
void la_dance(unsigned short move) {
|
| trmontgomery |
16:62976c3c029e
|
42
|
if (move & ra_mask == 0x100){
|
| trmontgomery |
16:62976c3c029e
|
43
|
l_arm.write(.50);
|
| trmontgomery |
16:62976c3c029e
|
44
|
} else {
|
| trmontgomery |
16:62976c3c029e
|
45
|
l_arm.write(.25);
|
| trmontgomery |
16:62976c3c029e
|
46
|
}
|
| trmontgomery |
16:62976c3c029e
|
47
|
|
| trmontgomery |
16:62976c3c029e
|
48
|
}
|
| trmontgomery |
16:62976c3c029e
|
49
|
|
| trmontgomery |
16:62976c3c029e
|
50
|
void rl_dance() {
|
| trmontgomery |
16:62976c3c029e
|
51
|
r_leg = 0;
|
| trmontgomery |
16:62976c3c029e
|
52
|
wait(1);
|
| trmontgomery |
16:62976c3c029e
|
53
|
for(float p=0; p<1.0; p += 0.1) {
|
| trmontgomery |
16:62976c3c029e
|
54
|
r_leg = p;
|
| trmontgomery |
16:62976c3c029e
|
55
|
wait(0.2);
|
| trmontgomery |
16:62976c3c029e
|
56
|
}
|
| trmontgomery |
16:62976c3c029e
|
57
|
r_leg.write(.50);
|
| trmontgomery |
16:62976c3c029e
|
58
|
|
| trmontgomery |
16:62976c3c029e
|
59
|
}
|
| trmontgomery |
16:62976c3c029e
|
60
|
|
| trmontgomery |
16:62976c3c029e
|
61
|
void ll_dance() {
|
| trmontgomery |
16:62976c3c029e
|
62
|
l_leg = 0;
|
| trmontgomery |
16:62976c3c029e
|
63
|
wait(1);
|
| trmontgomery |
16:62976c3c029e
|
64
|
for(float p=0; p<1.0; p += 0.1) {
|
| trmontgomery |
16:62976c3c029e
|
65
|
l_leg = p;
|
| trmontgomery |
16:62976c3c029e
|
66
|
wait(0.2);
|
| trmontgomery |
16:62976c3c029e
|
67
|
}
|
| trmontgomery |
16:62976c3c029e
|
68
|
l_leg.write(.50);
|
| trmontgomery |
16:62976c3c029e
|
69
|
|
| trmontgomery |
16:62976c3c029e
|
70
|
}
|
| trmontgomery |
16:62976c3c029e
|
71
|
|
| trmontgomery |
16:62976c3c029e
|
72
|
|
| trmontgomery |
16:62976c3c029e
|
73
|
void move_motors(){
|
| trmontgomery |
16:62976c3c029e
|
74
|
for (int i = 0; i < 12; ++i){
|
| trmontgomery |
16:62976c3c029e
|
75
|
ra_dance(move_map[i]);
|
| trmontgomery |
16:62976c3c029e
|
76
|
la_dance(move_map[i]);
|
| trmontgomery |
16:62976c3c029e
|
77
|
wait(0.2);
|
| trmontgomery |
16:62976c3c029e
|
78
|
}
|
| trmontgomery |
16:62976c3c029e
|
79
|
}
|
| trmontgomery |
16:62976c3c029e
|
80
|
|
| trmontgomery |
16:62976c3c029e
|
81
|
|
Carter Montgomery
