team10 team10 / AIviate1

imu rev1

Dependencies:   IMUfilter mbed

Fork of AIviate by UCLA IEEE

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?

UserRevisionLine numberNew 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 }