void RobotClass::updatePosition(RobotClass::sensorsAllowed sensors) {
deltaTime = millis() - lastTime;
delay(1);
if (sensors = RobotClass::sensorsAllowed::IMU_ENC) {
//Calculate distances based on encoders
calcRDistances();
//Calculate Velocities based on encoders
calcRVelocities(leftDistance, rightDistance, deltaTime);
//Update total distance counters
updateDistances(leftDistance, rightDistance, centerDistance);
//Second, read values from the IMU
readIMU();
//Calculate change in orientation using IMU
deltaTheta = (prevGyro.zRot + currGyro.zRot)*deltaTime/2000.0f;
//testing out the complimentary filter
//Update the robots pose
updateRobotPose(centerDistance, leftDistance, rightDistance, deltaTheta, deltaTime);
updateGlobalPose(centerDistance, leftDistance, rightDistance);
lcd.gotoXY(0, 0);
lcd.print("X:");
lcd.gotoXY(2, 0);
lcd.print(globalCurrentPos.x);
lcd.gotoXY(0, 1);
lcd.print("Y:");
lcd.gotoXY(2, 1);
lcd.print(globalCurrentPos.y);
}
lastTime = millis();
}
// Reads the various available sensors to determine if there is new data available.
// If new data is available it is downloaded from the sensor into the appropriate variable.
void TiltyIMU::updateSensors()
{
imu_updated = false;
altimeter_updated = false;
compass_updated = false;
if (hasIMU)
{
byte status = imu.getIntStatus();
//Serial.println(status, HEX);
if (status & 0x13)// Used to be 0x12, but new data register is 0x01, so set to 0x13 to meet both conditions
{
readIMU(status);
imu_updated = true;
}
}
if (hasAlt)
{
if (alt.getDataReady())
{
alt.readAltitudeM(&altimeter_data);
alt.readTempC(&altimeter_temp);
altimeter_updated = true;
}
}
if (hasMagn)
{
if (magn.getDataReady())
{
magn.getValues(raw_compass_data);
compass_updated = true;
}
}
}
int main(void)
{
PWM_Initialization();
TIM1->CCR2 = 1000;
TIM1->CCR3 = 1000;
Delay_1us(500000);
RCC_Configuration();
PushButton_Initialization();
LED_Initialization();
//LCD_Initialization();
//terminalBufferInitilization();
Delay_1us(100000);
SPI_Initialization();
Delay_1us(100000);
IMU_Initialization();
Delay_1us(100000);
Timer5_Initialization(); //Filter
Timer2_Initialization(); //Print
Timer4_Initialization(); //Read IMU
USART3_Configuration();
USART3_puts("\r\nHello World!\r\n");
while(1)
{
if(PushButton_Read()){
if(f.arm == 0){
f.arm = 1;
Delay_1us(500000);
}
else if(f.arm == 1){
f.arm = 0;
Delay_1us(500000);
}
}
if(f.imu == 1){
//LED3_Toggle();
readIMU(gyroRaw, GYRO_DATA_REGISTER);
gyro_remove_offset(gyroRaw);
readIMU(accRaw, ACC_DATA_REGISTER);
f.imu = 0;
// }
// if(f.filter == 1){
//LED4_Toggle();
Filter(gyroRaw, gyroAngle, accRaw, accAngle, Angle);
if(f.arm == 1){
PID_control(Angle);
}
else{
TIM1->CCR2 = 1000;
TIM1->CCR3 = 1000;
errorI = 0;
errorD = 0;
previous_error = 0;
}
//f.filter = 0;
}
strcmd_main();
//if(f.print == 1){
// sprintf(lcd_text_main,"%.4f %.4f %d", Angle[0], Angle[1], f.arm);
// //sprintf(lcd_text_main,"G: %.3f %.3f %.3f", EstV.G.X, EstV.G.Y, EstV.G.Z);
// LCD_DisplayStringLine(LINE(1), lcd_text_main);
// //sprintf(lcd_text_main,"A: %.3f %.3f %.3f", EstV.A.X, EstV.A.Y, EstV.A.Z);
// //sprintf(lcd_text_main,"A: %.3f %.3f", sqX_sqZ, EstV.GA.X*EstV.GA.X + EstV.GA.Z*EstV.GA.Z);
// // sprintf(lcd_text_main,"%.4f %.4f %.4f \n", gyroAngle[0], gyroAngle[1], gyroAngle[2]);
// sprintf(lcd_text_main,"%d ", gyroRaw[2]);
// LCD_DisplayStringLine(LINE(2), lcd_text_main);
// sprintf(lcd_text_main,"GA: %.3f %.3f %.3f", EstV.GA.X, EstV.GA.Y, EstV.GA.Z);
// LCD_DisplayStringLine(LINE(3), lcd_text_main);
//sprintf(lcd_text_main,"%.3f %.3f %.3f\n", EstV.G.Z, EstV.A.Z, EstV.GA.Z);
//LCD_DisplayStringLine(LINE(2), (uint8_t*)" Ming6842 @ github");
//terminalWrite(lcd_text_main);
//PRINT_USART();
//f.print = 0;
//}
}
while(1); // Don't want to exit
}
