team10 team10 / AIviate1

imu rev1

Dependencies:   IMUfilter mbed

Fork of AIviate by UCLA IEEE

steps.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 "steps.h"
teamgoat 0:0c627ff4c5ed 2
teamgoat 0:0c627ff4c5ed 3 Serial pc(USBTX, USBRX);
teamgoat 0:0c627ff4c5ed 4
teamgoat 0:0c627ff4c5ed 5 // in the future, change get_sensor_data to append the sensor data to a rolling list
teamgoat 4:44a5b1e8fd27 6 int get_sensor_data(int pid)
teamgoat 0:0c627ff4c5ed 7 {
teamgoat 2:452dd766d212 8 struct sensor s;
teamgoat 4:44a5b1e8fd27 9
teamgoat 4:44a5b1e8fd27 10 // retrieve process memory pointer
teamgoat 4:44a5b1e8fd27 11 char* mem = get_output(pid);
teamgoat 4:44a5b1e8fd27 12 int gx=0, gy=0, gz=0;
teamgoat 4:44a5b1e8fd27 13 if (sscanf(mem, "gx0 %i gy0 %i gz0 %i;\r\n", &gx, &gy, &gz) != 3)
teamgoat 4:44a5b1e8fd27 14 {
teamgoat 4:44a5b1e8fd27 15 // probably first time running process, retrieve data from init_sensors and store in current process output buffer
teamgoat 4:44a5b1e8fd27 16
teamgoat 4:44a5b1e8fd27 17 }
teamgoat 4:44a5b1e8fd27 18 else
teamgoat 4:44a5b1e8fd27 19 {
teamgoat 4:44a5b1e8fd27 20 s.gx0 = gx;
teamgoat 4:44a5b1e8fd27 21 s.gy0 = gy;
teamgoat 4:44a5b1e8fd27 22 s.gz0 = gz;
teamgoat 4:44a5b1e8fd27 23 }
teamgoat 4:44a5b1e8fd27 24
teamgoat 4:44a5b1e8fd27 25
teamgoat 0:0c627ff4c5ed 26 if (read_accelerometer(&s) == 0)
teamgoat 0:0c627ff4c5ed 27 {
teamgoat 4:44a5b1e8fd27 28 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 29 pc.printf("Error in get_sensor_data while reading from accel!\r\n");
teamgoat 4:44a5b1e8fd27 30 return 1;
teamgoat 0:0c627ff4c5ed 31 }
teamgoat 3:f9e18a9cd9af 32 if (read_gyro(&s) == 0)
teamgoat 3:f9e18a9cd9af 33 {
teamgoat 4:44a5b1e8fd27 34 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 35 pc.printf("Error in get_sensor_data while reading from gyro!\r\n");
teamgoat 4:44a5b1e8fd27 36 return 1;
teamgoat 3:f9e18a9cd9af 37 }
team10 5:d85bac38cdff 38 // updated output format for matlab
teamgoat 4:44a5b1e8fd27 39 if (USBDEBUG)
team10 5:d85bac38cdff 40 //pc.printf("%i,%i,%i\r\n", s.ax, s.ay, s.az);
team10 5:d85bac38cdff 41 pc.printf("%i,%i,%i,%i,%i,%i\r\n", s.ax, s.ay, s.az, s.gx, s.gy, s.gz);
team10 5:d85bac38cdff 42
team10 5:d85bac38cdff 43
team10 5:d85bac38cdff 44 //if (USBDEBUG)
team10 5:d85bac38cdff 45 // pc.printf("Ax: %i Ay: %i Az: %i Gx: %i Gy: %i Gz: %i\r\n", s.ax, s.ay, s.az, s.gx, s.gy, s.gz);
teamgoat 4:44a5b1e8fd27 46 return 1;
teamgoat 2:452dd766d212 47 }
teamgoat 2:452dd766d212 48
teamgoat 4:44a5b1e8fd27 49 int init_sensors(int pid)
teamgoat 2:452dd766d212 50 {
teamgoat 2:452dd766d212 51 // create config struct
teamgoat 2:452dd766d212 52 struct config c;
teamgoat 2:452dd766d212 53
teamgoat 2:452dd766d212 54 // set configurations
teamgoat 2:452dd766d212 55 c.frequency = 10000;
teamgoat 2:452dd766d212 56
teamgoat 2:452dd766d212 57 // pass configuration struct to configuration routine
teamgoat 2:452dd766d212 58 int ret = config_gy80(&c);
teamgoat 2:452dd766d212 59 if (ret == 0)
teamgoat 2:452dd766d212 60 {
teamgoat 4:44a5b1e8fd27 61 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 62 pc.printf("Error configuring sensors\r\n");
teamgoat 2:452dd766d212 63 }
teamgoat 4:44a5b1e8fd27 64
teamgoat 4:44a5b1e8fd27 65 // accumulations of gyro values (for zeroing)
teamgoat 4:44a5b1e8fd27 66 int gx = 0, gy = 0, gz = 0;
teamgoat 4:44a5b1e8fd27 67 struct sensor s;
teamgoat 4:44a5b1e8fd27 68 int numzerotrials = 10;
teamgoat 4:44a5b1e8fd27 69 for (int i=0; i<numzerotrials; i++)
teamgoat 4:44a5b1e8fd27 70 {
teamgoat 4:44a5b1e8fd27 71 if (read_gyro(&s) == 0)
teamgoat 4:44a5b1e8fd27 72 {
teamgoat 4:44a5b1e8fd27 73 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 74 pc.printf("Error in collecting zero-level data from gyro (init_sensors)\r\n");
teamgoat 4:44a5b1e8fd27 75 return 1;
teamgoat 4:44a5b1e8fd27 76 }
teamgoat 4:44a5b1e8fd27 77 gx += s.gx;
teamgoat 4:44a5b1e8fd27 78 gy += s.gy;
teamgoat 4:44a5b1e8fd27 79 gz += s.gz;
teamgoat 4:44a5b1e8fd27 80 }
teamgoat 4:44a5b1e8fd27 81
teamgoat 4:44a5b1e8fd27 82 gx /= numzerotrials;
teamgoat 4:44a5b1e8fd27 83 gy /= numzerotrials;
teamgoat 4:44a5b1e8fd27 84 gz /= numzerotrials;
teamgoat 4:44a5b1e8fd27 85
teamgoat 4:44a5b1e8fd27 86 char * output = get_output(pid);
teamgoat 4:44a5b1e8fd27 87 sprintf(output, "gx0 %i gy0 %i gz0 %i;\r\n", gx, gy, gz);
teamgoat 4:44a5b1e8fd27 88
teamgoat 4:44a5b1e8fd27 89 schedule_proc("RETDATA", &get_sensor_data);
teamgoat 4:44a5b1e8fd27 90 return 0;
teamgoat 4:44a5b1e8fd27 91 }