imu rev1
Fork of AIviate by
control.cpp@5:d85bac38cdff, 2013-11-26 (annotated)
- Committer:
- team10
- Date:
- Tue Nov 26 20:29:54 2013 +0000
- Revision:
- 5:d85bac38cdff
- Parent:
- 4:44a5b1e8fd27
imu rev1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
teamgoat | 0:0c627ff4c5ed | 1 | #include "control.h" |
teamgoat | 0:0c627ff4c5ed | 2 | #include "sensor.h" |
teamgoat | 0:0c627ff4c5ed | 3 | #include "steps.h" |
teamgoat | 0:0c627ff4c5ed | 4 | #include "mbed.h" |
team10 | 5:d85bac38cdff | 5 | #include "IMUfilter.h" |
teamgoat | 0:0c627ff4c5ed | 6 | |
teamgoat | 0:0c627ff4c5ed | 7 | #define MAXPROC 15 |
team10 | 5:d85bac38cdff | 8 | IMUfilter imuFilter(0.1, 10); |
team10 | 5:d85bac38cdff | 9 | |
teamgoat | 0:0c627ff4c5ed | 10 | |
teamgoat | 0:0c627ff4c5ed | 11 | process procs[MAXPROC] = {0}; |
team10 | 5:d85bac38cdff | 12 | Serial pc3(USBTX, USBRX); |
team10 | 5:d85bac38cdff | 13 | LocalFileSystem local("local"); |
teamgoat | 0:0c627ff4c5ed | 14 | int main() |
teamgoat | 0:0c627ff4c5ed | 15 | { |
team10 | 5:d85bac38cdff | 16 | |
teamgoat | 2:452dd766d212 | 17 | init(); |
team10 | 5:d85bac38cdff | 18 | pc3.baud(115200); |
team10 | 5:d85bac38cdff | 19 | accelerometer_measure(); |
team10 | 5:d85bac38cdff | 20 | gyro_turnon(); |
team10 | 5:d85bac38cdff | 21 | sensor ter; |
team10 | 5:d85bac38cdff | 22 | int a; |
teamgoat | 0:0c627ff4c5ed | 23 | while (true) |
teamgoat | 0:0c627ff4c5ed | 24 | { |
teamgoat | 0:0c627ff4c5ed | 25 | schedule(); |
team10 | 5:d85bac38cdff | 26 | a = read_accelerometer(&ter); |
team10 | 5:d85bac38cdff | 27 | a = read_gyro(&ter); |
team10 | 5:d85bac38cdff | 28 | |
team10 | 5:d85bac38cdff | 29 | wait(1); |
team10 | 5:d85bac38cdff | 30 | //Gyro and accelerometer values must be converted to rad/s and m/s/s |
team10 | 5:d85bac38cdff | 31 | //before being passed to the filter. |
team10 | 5:d85bac38cdff | 32 | /* imuFilter.updateFilter(ter.gx, |
team10 | 5:d85bac38cdff | 33 | ter.gy, |
team10 | 5:d85bac38cdff | 34 | ter.gz, |
team10 | 5:d85bac38cdff | 35 | ter.ax, |
team10 | 5:d85bac38cdff | 36 | ter.ay, |
team10 | 5:d85bac38cdff | 37 | ter.az); |
team10 | 5:d85bac38cdff | 38 | imuFilter.computeEuler(); |
team10 | 5:d85bac38cdff | 39 | printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw()); |
team10 | 5:d85bac38cdff | 40 | |
team10 | 5:d85bac38cdff | 41 | */ |
team10 | 5:d85bac38cdff | 42 | |
team10 | 5:d85bac38cdff | 43 | |
teamgoat | 2:452dd766d212 | 44 | } |
team10 | 5:d85bac38cdff | 45 | ; |
teamgoat | 0:0c627ff4c5ed | 46 | } |
teamgoat | 0:0c627ff4c5ed | 47 | |
teamgoat | 2:452dd766d212 | 48 | void init() |
teamgoat | 0:0c627ff4c5ed | 49 | { |
teamgoat | 2:452dd766d212 | 50 | // initialize i2c sensors |
teamgoat | 4:44a5b1e8fd27 | 51 | schedule_proc("INITSENSE", &init_sensors); |
teamgoat | 2:452dd766d212 | 52 | // set initial processes |
teamgoat | 0:0c627ff4c5ed | 53 | procs[0].status = READY; |
teamgoat | 0:0c627ff4c5ed | 54 | procs[0].start = &get_sensor_data; |
teamgoat | 0:0c627ff4c5ed | 55 | return; |
teamgoat | 0:0c627ff4c5ed | 56 | } |
teamgoat | 0:0c627ff4c5ed | 57 | |
teamgoat | 4:44a5b1e8fd27 | 58 | char* get_output(int pid) |
teamgoat | 4:44a5b1e8fd27 | 59 | { |
teamgoat | 4:44a5b1e8fd27 | 60 | return procs[pid].output; |
teamgoat | 4:44a5b1e8fd27 | 61 | } |
teamgoat | 4:44a5b1e8fd27 | 62 | |
teamgoat | 0:0c627ff4c5ed | 63 | void schedule() |
teamgoat | 0:0c627ff4c5ed | 64 | { |
teamgoat | 0:0c627ff4c5ed | 65 | for (int i=0; i<MAXPROC; i++) |
teamgoat | 0:0c627ff4c5ed | 66 | { |
teamgoat | 0:0c627ff4c5ed | 67 | process proc = procs[i]; |
teamgoat | 0:0c627ff4c5ed | 68 | if(proc.status == READY) |
teamgoat | 0:0c627ff4c5ed | 69 | { |
teamgoat | 4:44a5b1e8fd27 | 70 | int ret = proc.start(i); |
teamgoat | 4:44a5b1e8fd27 | 71 | // if the process returns 0, it means don't run again |
teamgoat | 4:44a5b1e8fd27 | 72 | if (ret == 0) |
teamgoat | 4:44a5b1e8fd27 | 73 | { |
teamgoat | 4:44a5b1e8fd27 | 74 | // set proc.status to ZOMBIE |
teamgoat | 4:44a5b1e8fd27 | 75 | // ZOMBIE means process |
teamgoat | 4:44a5b1e8fd27 | 76 | // is no longer active, but |
teamgoat | 4:44a5b1e8fd27 | 77 | // it's output buffer is still |
teamgoat | 4:44a5b1e8fd27 | 78 | // needed. |
teamgoat | 4:44a5b1e8fd27 | 79 | proc.status = ZOMBIE; |
teamgoat | 4:44a5b1e8fd27 | 80 | } |
teamgoat | 0:0c627ff4c5ed | 81 | return; |
teamgoat | 0:0c627ff4c5ed | 82 | } |
teamgoat | 0:0c627ff4c5ed | 83 | } |
teamgoat | 0:0c627ff4c5ed | 84 | } |
teamgoat | 4:44a5b1e8fd27 | 85 | |
teamgoat | 4:44a5b1e8fd27 | 86 | int schedule_proc(char *sid, int (*funcptr)(int)) |
teamgoat | 4:44a5b1e8fd27 | 87 | { |
teamgoat | 4:44a5b1e8fd27 | 88 | for (int i=0; i<MAXPROC; i++) |
teamgoat | 4:44a5b1e8fd27 | 89 | { |
teamgoat | 4:44a5b1e8fd27 | 90 | process proc = procs[i]; |
teamgoat | 4:44a5b1e8fd27 | 91 | if(proc.status == EMPTY) |
teamgoat | 4:44a5b1e8fd27 | 92 | { |
teamgoat | 4:44a5b1e8fd27 | 93 | proc.status = READY; |
teamgoat | 4:44a5b1e8fd27 | 94 | proc.start = funcptr; |
teamgoat | 4:44a5b1e8fd27 | 95 | strncpy(proc.sid, sid, MAX_SID_LEN-1); |
teamgoat | 4:44a5b1e8fd27 | 96 | // null terminate proc.sid |
teamgoat | 4:44a5b1e8fd27 | 97 | proc.sid[MAX_SID_LEN-1] = 0; |
teamgoat | 4:44a5b1e8fd27 | 98 | return i; |
teamgoat | 4:44a5b1e8fd27 | 99 | } |
teamgoat | 4:44a5b1e8fd27 | 100 | } |
teamgoat | 4:44a5b1e8fd27 | 101 | // if no open processes, return -1 |
teamgoat | 4:44a5b1e8fd27 | 102 | return -1; |
teamgoat | 4:44a5b1e8fd27 | 103 | } |