// Echoes anything received via UART2 using interrupts
void echo_task(void *pvParameters)
{
while(1) {
while(!c_common_usart_available(USART2)) { vTaskDelay(10/portTICK_RATE_MS); }
c_common_usart_puts(USART2, "Got: ");
c_common_usart_putchar(USART2, c_common_usart_read(USART2));
c_common_usart_puts(USART2, " \n\r");
}
}
// Prints what the receiver gets from the remote
void uart_task(void *pvParameters)
{
char str[32];
while(1) {
c_common_usart_puts(USART2, "\n\r---------------------\n\r");
for(int i=0; i<6; i++) {
sprintf(str, "Canal %d : %d\n\r", i, c_rc_receiver_get_channel(i));
c_common_usart_puts(USART2, str);
}
vTaskDelay(1000/portTICK_RATE_MS);
}
}
/* Main ----------------------------------------------------------------------*/
int main(void)
{
SystemInit();
prvHardwareInit();
vTraceInitTraceData();
c_common_usart_puts(USART2, "Programa iniciado!\n\r");
vTraceConsoleMessage("Starting application...");
//sprintf(str, "Line, %d \n\r", __LINE__);
//c_common_usart_puts(USART2, str);
if (! uiTraceStart() )
vTraceConsoleMessage("Could not start recorder!");
/* create tasks */
xTaskCreate(blink_led_task, (signed char *)"Blink led", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
xTaskCreate(sonar_task, (signed char *)"Sonar task", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
//xTaskCreate(echo_task, (signed char *)"Echo task", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
//xTaskCreate(blctrl_task, (signed char *)"blctrl task" , configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
//xTaskCreate(uart_task , (signed char *)"UART task", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
//xTaskCreate(rc_servo_task , (signed char *)"Servo task", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY+1, NULL);
/* Start the scheduler. */
vTaskStartScheduler();
/* should never reach here! */
for(;;);
}
// Periodically reads the IMU and outputs the raw content via UART2
void i2c_task(void *pvParameters)
{
char str[64];
c_common_i2c_readBytes(ADXL345_ADDR, 0x00, 1, &ADXL345_ID);
// Accelerometer increase G-range (+/- 16G)
c_common_i2c_writeByte(ADXL345_ADDR, 0x31, 0b00001011);
// ADXL345 POWER_CTL
c_common_i2c_writeByte(ADXL345_ADDR, 0x2D, 0);
c_common_i2c_writeByte(ADXL345_ADDR, 0x2D, 16);
c_common_i2c_writeByte(ADXL345_ADDR, 0x2D, 8);
while(1) {
// Read x, y, z acceleration, pack the data.
c_common_i2c_readBytes(ADXL345_ADDR, ADXL345_X_ADDR, 6, sensorBuffer);
accRaw[0] = ((int)sensorBuffer[0] | ((int)sensorBuffer[1] << 8)) * -1;
accRaw[1] = ((int)sensorBuffer[2] | ((int)sensorBuffer[3] << 8)) * -1;
accRaw[2] = (int)sensorBuffer[4] | ((int)sensorBuffer[5] << 8);
sprintf(str, "Accel: %d %d %d\n\r", accRaw[0], accRaw[1], accRaw[2]);
c_common_usart_puts(USART2, str);
vTaskDelay(100/portTICK_RATE_MS);
}
}
void blctrl_task(void *pvParameters)
{
char str[30];
c_io_blctrl_setSpeed(BLCTRL_ADDR, 0);
while(1)
{
vTaskDelay(10/portTICK_RATE_MS);
c_io_blctrl_updateBuffer(BLCTRL_ADDR);
sprintf(str, "esc rpm: %d \n\r",(int)c_io_blctrl_readSpeed(BLCTRL_ADDR) );
c_common_usart_puts(USART2, str);
sprintf(str, "esc Voltage: %d \n\r",(int)c_io_blctrl_readVoltage(BLCTRL_ADDR) );
c_common_usart_puts(USART2, str);
vTaskDelay(100/portTICK_RATE_MS);
c_io_blctrl_setSpeed(BLCTRL_ADDR, 195);
}
}
void sonar_task(void *pvParameters)
{
while(1)
{
char str[64];
sprintf(str, "Distance: %d \n\r", c_io_sonar_read());
c_common_usart_puts(USART2, str);
vTaskDelay(300/portTICK_RATE_MS);
}
}
/** \brief Inicializacao componentes de IO.
*
* Incializa o hardware para comunicar com os sensores. Rotinas de teste
* ainda precisam ser executadas.
* @param None
* @retval None
*/
void module_imu_init()
{
I2Cn=I2C1;
/* Inicialização do hardware do módulo */
LED4 = c_common_gpio_init(GPIOD, GPIO_Pin_12, GPIO_Mode_OUT); //LED4
/* Inicialização da imu */
//c_common_i2c_init(I2Cn);
c_io_imuAdafruit_init(I2Cn);
//c_io_imu_config(ADAFRUIT);
//imu_init(I2Cn);
c_common_usart2_init(BAUDRATE);
c_common_usart_puts(USARTn,"Adafruit 10DOF Tester");
c_common_usart_putchar(USARTn,'\n');
/* Initialise the sensors */
/*
if(!c_io_lsm303_initAccel(I2Cn)) {
// There was a problem detecting the ADXL345 ... check your connections
c_common_usart_puts(USARTn,"Ooops, no LSM303 detected ... Check your wiring!");
c_common_usart_putchar(USARTn,'\n');
while(1);
}
if(!c_io_lsm303_initMag(I2Cn)) {
// There was a problem detecting the LSM303 ... check your connections
c_common_usart_puts(USARTn,"Ooops, no LSM303 detected ... Check your wiring!");
c_common_usart_putchar(USARTn,'\n');
while(1);
}
if(!c_io_bmp085_init(I2Cn,BMP085_MODE_ULTRAHIGHRES)) {
// There was a problem detecting the BMP085 ... check your connections
c_common_usart_puts(USARTn,"Ooops, no BMP085 detected ... Check your wiring or I2C ADDR!");
while(1);
}
if(!c_io_l3gd20_init(I2Cn,GYRO_RANGE_250DPS)) {
// There was a problem detecting the L3GD20 ... check your connections
c_common_usart_puts(USARTn,"Ooops, no L3GD20 detected ... Check your wiring or I2C ADDR!");
while(1);
}
*/
/* Display some basic information on this sensor */
//displaySensorDetails();
}
void displayDataDetails(float *accel, float *mag, float *gyro)
{
char tmp[20];
//c_io_l3gd20_getGyroData(gyro);
//c_io_lsm303_getAccelData(accel);
//c_io_lsm303_getMagData(mag);
c_common_usart_puts(USARTn,"Acelerometro:\nx: ");
c_common_utils_floatToString(accel[0],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\ny: ");
c_common_utils_floatToString(accel[1],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\nz: ");
c_common_utils_floatToString(accel[2],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\n\n");
c_common_usart_puts(USARTn,"Magnetometro:\nx: ");
c_common_utils_floatToString(mag[0],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\ny: ");
c_common_utils_floatToString(mag[1],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\nz: ");
c_common_utils_floatToString(mag[2],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\n\n");
c_common_usart_puts(USARTn,"Giroscopio:\nx: ");
c_common_utils_floatToString(gyro[0],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\ny: ");
c_common_utils_floatToString(gyro[1],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\nz: ");
c_common_utils_floatToString(gyro[2],tmp,6);
c_common_usart_puts(USARTn,tmp);
c_common_usart_puts(USARTn,"\n\n");
}
