void imu_main_task(void *param)
{
xSemaphore_pressure_temperature_mutex_handle = xSemaphoreCreateMutex();
if(xSemaphoreTake(xSemaphore_pressure_temperature_mutex_handle, 0))
{
temperature = 0.0;
pressure = 0.0;
xSemaphoreGive( xSemaphore_pressure_temperature_mutex_handle );
}
setup_imu();
while(true)
{
// loop_imu();
vTaskDelay(20/portTICK_RATE_MS); /* wait for 10ms (100Hz) */
} /* for */
}
//***************** You can start coding here *****************//
int main (void)
{
// MRF SPI Init
uint8_t channel = 12;
print_adxl_timer.start();
mrf.SetChannel(channel); //Set the Channel. 0 is default, 15 is max
// ///////////
// M2 SPI Init
ss = 1;
spi.format(8, 0);
//spi.format(16, 0);
spi.frequency(SPI_FREQ);
spi_start_ticker.attach_us(&SPI_start_handler, SPI_PRD*1000);
read_adxl = false;
// ///////////
// IMU I2C Init
i2c.frequency(400000); // 400KHz
setup_imu();
// ///////////
//Start the timer
timer.start();
debounce.start();
strcpy(testString, "0,2,0.2,1,0.6,0.7,0.7,0.4,0");
err();
testcheck = 1;
button.rise(debounce_handler);
training_mode = false;
// Initial Lights
recMode = 0;
// Timer initiate
for (int i = 0; i < 7; i++) {
//timerarray[i].start();
timerarray[i].stop();
}
// Digital Pin Pwms
for (int i = 0; i < 7; i++) {
pwm_duty_cycles[i] = 0.0;
}
ticker_30Hz.attach_us(&ticker_30Hz_handler, THIRTYHZ*1000);
pc.printf("Starting...\n");
while(1) {
greenlightact();
if (training_mode) {
if (read_adxl) read_adxl_from_m2();
if (print_adxl_timer.read_ms() > PRINT_PRD) {
print_adxl_timer.reset();
pc.printf("dx: ");
for (int i = 0; i < NUM_ADXL; i++)
pc.printf("%d\t", adxl[i]);
pc.printf("\tdtheta: ");
for (int i = 0; i < NUM_IMU; i++)
pc.printf("%d\t", imu[i]);
pc.printf("\t");
for (int i = 0; i < 7; i++)
pc.printf("%d\t", vhit[i]);
pc.printf("\n");
}
// GUI controlled
//TODO TODO TODO
if (pc.readable()) {
pc.gets(gui_input_buf, 16);
sscanf(gui_input_buf, "%d,%d,%d,%d,%d,%d,%d,%d", &recMode,green,green+1,green+2,green+3,green+4,green+5,green+6);
pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\n", recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
//pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\r\n", *recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
if (recMode > 0) {
trigger(recMode);
}
}
pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\n", recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
}
else {
//Try to receive some data
rxLen = rf_receive(rxBuffer, 128);
if(rxLen > 0) {
pc.printf("%s \r\n", rxBuffer );
token = strtok(rxBuffer, " ,");
player = atoi(token);
if (player == LINKED_PLAYER) {
token = strtok(NULL, " ,");
inputs = atoi(token);
for (int i = 0; i < inputs; i++) {
token = strtok(NULL, ",");
float temp = atof(token);
//float temp = ((float) atoi(token))/MAX_ACCEL_VAL;
temp = 1.0 - temp;
pwm_duty_cycles[i] = temp;
if (temp > 0.2) {
green[i] = 0;
}
else {
green[i] = 1;
}
}
}
}
}
}
}
