int main ( void )
{
/* Initialize all MPLAB Harmony modules, including application(s). */
SYS_Initialize ( NULL );
__builtin_disable_interrupts();
// set the CP0 CONFIG register to indicate that kseg0 is cacheable (0x3)
__builtin_mtc0(_CP0_CONFIG, _CP0_CONFIG_SELECT, 0xa4210583);
// 0 data RAM access wait states
BMXCONbits.BMXWSDRM = 0x0;
// enable multi vector interrupts
INTCONbits.MVEC = 0x1;
// disable JTAG to get pins back
DDPCONbits.JTAGEN = 0;
// initialize I2C
initI2C2();
// initialize IMU
initIMU();
// initialize SPI, LCD
// SPI1_init();
// LCD_init();
__builtin_enable_interrupts();
// LCD_clearScreen(BLACK);
// char message[MAX_LENGTH];
unsigned char data[14];
// float accX, accY;
_CP0_SET_COUNT(0);
while ( true )
{
if (_CP0_GET_COUNT() > 480000) { // 50 Hz
i2c_read_multiple(IMU_ADDRESS, OUT_TEMP_L, data, 14);
temp_raw = data[1] << 8 | data[0];
gyroX_raw = data[3] << 8 | data[2];
gyroY_raw = data[5] << 8 | data[4];
gyroZ_raw = data[7] << 8 | data[6];
accX_raw = data[9] << 8 | data[8];
accY_raw = data[11] << 8 | data[10];
accZ_raw = data[13] << 8 | data[12];
// accX = accX_raw * 2.0 / 32768; // accel in g
// accY = accY_raw * 2.0 / 32768; // accel in g
// accZ = accZ_raw * 2.0 / 32768; // accel in g
// sprintf(message, "temp raw: %x ", temp_raw);
// LCD_drawString(10, 10, message, WHITE);
//
// sprintf(message, "accX: %f g ", accX);
// LCD_drawString(10, 20, message, WHITE);
//
// sprintf(message, "accY: %f g ", accY);
// LCD_drawString(10, 30, message, WHITE);
//
// sprintf(message, "accZ: %f g ", accZ);
// LCD_drawString(10, 40, message, WHITE);
//
// sprintf(message, "gyroX raw: %i ", gyroX_raw);
// LCD_drawString(10, 50, message, WHITE);
//
// sprintf(message, "gyroY raw: %i ", gyroY_raw);
// LCD_drawString(10, 60, message, WHITE);
//
// sprintf(message, "gyroZ raw: %i ", gyroZ_raw);
// LCD_drawString(10, 70, message, WHITE);
_CP0_SET_COUNT(0);
}
/* Maintain state machines of all polled MPLAB Harmony modules. */
SYS_Tasks ( );
}
/* Execution should not come here during normal operation */
return ( EXIT_FAILURE );
}
/**
* \brief Initialization function
*
* \param [in] nav_data Parameter_Description
* \return None
*
* \details Initializes navigation data structure
*/
void initNav(nav_struct* nav_data)
{
if(ENABLE_IMU)
{
/// begin I2C transmission with sensors
Wire.begin();
/// Initialize IMU sensors
initIMU();
}
/// initialize Estimated navigation states
nav_data->vbbx = 0.0;
nav_data->vbby = 0.0;
nav_data->vbbz = 0.0;
nav_data->pbbx = 0.0;
nav_data->pbby = 0.0;
nav_data->pbbz = 0.0;
nav_data->Ebbx = 0.0;
nav_data->Ebby = 0.0;
nav_data->Ebbz = 0.0;
/// initialize rotation matrix (inertial to body)
nav_data->dcm_eb[0][0] = 1.0;
nav_data->dcm_eb[1][0] = 0.0;
nav_data->dcm_eb[2][0] = 0.0;
nav_data->dcm_eb[0][1] = 0.0;
nav_data->dcm_eb[1][1] = 1.0;
nav_data->dcm_eb[2][1] = 0.0;
nav_data->dcm_eb[0][2] = 0.0;
nav_data->dcm_eb[1][2] = 0.0;
nav_data->dcm_eb[2][2] = 1.0;
/// initialize GPS fix values
nav_data->gps_lan.gps_fix = false;
nav_data->gps_lan.gps_time = 0.0;
nav_data->gps_lan.lat_deg = 0.0;
nav_data->gps_lan.long_deg = 0.0;
nav_data->gps_lan.alt = 0.0;
nav_data->gps_lan.course = 0.0;
nav_data->gps_lan.speed = 0.0;
nav_data->gps_first_fix.gps_fix = false;
nav_data->gps_first_fix.gps_time = 0.0;
nav_data->gps_first_fix.lat_deg = 0.0;
nav_data->gps_first_fix.long_deg = 0.0;
nav_data->gps_first_fix.alt = 0.0;
nav_data->gps_first_fix.course = 0.0;
nav_data->gps_first_fix.speed = 0.0;
/// initialize IMU values
nav_data->imu_lan.abbx = 0.0;
nav_data->imu_lan.abby = 0.0;
nav_data->imu_lan.abbz = 0.0;
nav_data->imu_lan.wbbx = 0.0;
nav_data->imu_lan.wbby = 0.0;
nav_data->imu_lan.wbbz = 0.0;
nav_data->imu_lan.imu_dT = 0.0;
/// initialize drift controller
nav_data->drift_lan.wbbx_err = 0.0;
nav_data->drift_lan.wbby_err = 0.0;
nav_data->drift_lan.wbbz_err = 0.0;
}
int main (void)
{
system_init();
// Initialize local variables used in main
int failed = 0;
uint8_t debug_string[24] = "y: x: z: c: ";
uint8_t jx, jy;
char z, c, lz, lc;
jx = jy = z = c = lz = lc = 0;
int cycle_index = 0;
uint8_t cycle = 0;
uint8_t light_mode = 0;
int light_modes = 1;
uint16_t head, brake = 0;
uint8_t LIGHTS_ON = 0;
uint8_t FWD = 0;
uint8_t HEADLIGHTS = 0;
uint8_t DEBUG_BLE = 1;
uint16_t BLE_temp = '0';
//uint16_t light_sens;
// Configure Devices
configure_port_pins();
configure_LED_PWM();
configure_usart();
configure_i2c_slave();
initIMU();
failed = !beginIMU();
configure_i2c_slave_callbacks();
//configure_ADC();
// Set the BLE module name to "long-itude"
uint8_t string[17] = "AT+NAMElong-itude";
usart_write_buffer_wait(&usart_instance, string, sizeof(string));
for(int i = 0; i < 50000; ++i);
// Switch LED direction to FWD
setFWD();
while(1)
{
//readAccel();
//readGyro();
//readMag();
//light_sens = getLightSens();
// Read data from BLE
if (usart_read_wait(&usart_instance, &BLE_temp) == STATUS_OK) {
switch(BLE_temp)
{
case '0':
LIGHTS_ON = 0;
break;
case '1':
LIGHTS_ON = 1;
break;
case '2':
DEBUG_BLE = 0;
break;
case '3':
DEBUG_BLE = 1;
break;
case '4':
setFWD();
break;
case '5':
setREV();
break;
}
}
if(DEBUG_BLE)
{
uint8_t temp = I2C_slave_read_buffer[0];
debug_string[4] = '0' + temp%10;
temp = (temp - temp%10)/10;
debug_string[3] = '0' + temp%10;
temp = (temp - temp%10)/10;
debug_string[2] = '0' + temp%10;
temp = (temp - temp%10)/10;
temp = I2C_slave_read_buffer[1];
debug_string[11] = '0' + temp%10;
temp = (temp - temp%10)/10;
debug_string[10] = '0' + temp%10;
temp = (temp - temp%10)/10;
debug_string[9] = '0' + temp%10;
temp = (temp - temp%10)/10;
debug_string[16] = '0' + I2C_slave_read_buffer[2];
debug_string[21] = '0' + I2C_slave_read_buffer[3];
debug_string[22] = '\r';
debug_string[23] = '\n';
usart_write_buffer_wait(&usart_instance, debug_string, sizeof(debug_string));
}
jx = I2C_slave_read_buffer[0];
jy = I2C_slave_read_buffer[1];
z = I2C_slave_read_buffer[2];
c = I2C_slave_read_buffer[3];
if(z == 0 && lz != 0)
{
if(jy > 200)
{
setFWD();
if(HEADLIGHTS && FWD)
HEADLIGHTS = 0;
else if(!HEADLIGHTS && FWD)
HEADLIGHTS = 1;
else if(HEADLIGHTS && !FWD)
{
FWD = 1;
}
else if(!HEADLIGHTS && !FWD)
{
HEADLIGHTS = 1;
FWD = 1;
}
}
else if(jy < 55)
{
setREV();
if(HEADLIGHTS && !FWD)
HEADLIGHTS = 0;
else if(!HEADLIGHTS && !FWD)
HEADLIGHTS = 1;
else if(HEADLIGHTS && FWD)
{
FWD = 0;
}
else if(!HEADLIGHTS && FWD)
{
HEADLIGHTS = 1;
FWD = 0;
}
}
else if(jy > 110 && jy < 150 && jx > 110 && jx < 150)
{
LIGHTS_ON = !LIGHTS_ON;
}
else if(jx > 200)
{
light_mode++;
if(light_mode >= light_modes)
light_mode = 0;
}
else if(jx < 55)
{
light_mode--;
if(light_mode <= -1)
light_mode = light_modes-1;
}
}
if(LIGHTS_ON)
{
if(light_mode == 0)
{
if(cycle == 0)
{
setLeftRGB(cycle_index,0,0xCFFF-cycle_index);
setRightRGB(cycle_index,0,0xCFFF-cycle_index);
}
if(cycle == 1)
{
setLeftRGB(0xCFFF-cycle_index,cycle_index,0);
setRightRGB(0xCFFF-cycle_index,cycle_index,0);
}
if(cycle == 2)
{
setLeftRGB(0,0xCFFF-cycle_index,cycle_index);
setRightRGB(0,0xCFFF-cycle_index,cycle_index);
}
cycle_index += 250;
if(cycle_index >= 0xCFFF)
{
cycle_index = 0;
cycle += 1;
if(cycle == 3)
cycle = 0;
}
}
if(HEADLIGHTS)
{
head = 0xFFFF;
setWhite(head);
if(jy < 120)
{
brake = (0xFFFF/120)*(120-jy);
setRed(brake);
}
else
setRed(0);
}
else
{
setWhite(0);
setRed(0);
}
}
else
{
setWhite(0);
setRed(0);
setLeftRGB(0,0,0);
setRightRGB(0,0,0);
}
lc = c;
lz = z;
}
}
/*
* Application entry point.
*/
int main(void) {
int8_t accelData[2]={0,0}; // Discovery Board's Accelerometer
uint8_t receivedBuff[4]={0,0,0,0}; // Received request/information from PandaBoard
uint8_t sentData[4] = {0,0,0,0}; // Returned Information (reply) to the PandaBoard
float imuData[7]={0,0,0,0,0,0,0}; // IMU calculated data based on Razor Boad
int* razorInfo; // Razor Board Data
int steering = 0;
int speed = 0;
int ir_data[3]={0,0,0};
int16_t us_data[3]={0,0,0};
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
mypwmInit();
// Initializing Motor
motorInit();
// Initializing IR Thread
ADCinit();
// Initializing US Thread
// myUltrasonicInit();
// Initializing Discovery Board's Accelerometer
//mySPIinit();
// Initializing Razor Board
myRazorInit();
// Activates the USB driver and then the USB bus pull-up on D+.
myUSBinit();
// Initializing IMU Calculations.
initIMU();
//Starting the usb configuration
sdStart(&SDU1,&portConfig2);
char receivedInfo[11];
/*
* Main loop, it takes care of reciving the requests from Panda Board using USB protocol,
* and reply with the requested data.
*/
while (TRUE) {
receivedInfo[0]='T';
sdRead(&SDU1, receivedInfo, 10);
// getImuValues(imuData);
// getAccel(accelData);
// getIR(ir_data);
// getUS(us_data);
if(receivedInfo[0] != 'T'){
receivedInfo[11]='\0';
parse(receivedInfo);
//setMotorData(-(rcvData[1]-28),rcvData[2]-2);
setMotorData(rcvData[1],1550);
translate(rcvData[0],ir_data,us_data,razorInfo,imuData,accelData,sentData);
sdWrite(&SDU1, sentData, 4);
}
}
}
