/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
uint16_t pulse_width = 0;
/* TIM Configuration */
TIM_Config();
/* LED Configuration */
LED_Config();
/* PWM Configuration */
PWM_Config(MAX_PERIOD);
PWM_SetDC(1,100);
PWM_SetDC(2,100);
PWM_SetDC(3,100);
PWM_SetDC(4,100);
while (1)
{
/* PD12 to be toggled */
// GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
PWM_SetDC(1,pulse_width++);
if (pulse_width > MAX_PERIOD / 10)
{
pulse_width = MAX_PERIOD / 20;
}
Delay(900000);
}
}
void platformFineAdjust(void) {
float diff = 0.0;
uint16_t ticks = getOrderCurrentTicks();
if(checkRightWall()) {
//_stop();
process_sensors();
diff = getIRSensorReadingCM(IR_SENSOR_RF)-getIRSensorReadingCM(IR_SENSOR_RB);
if(diff <= -MOTOR_ADJUSTMENT_CM || diff >= MOTOR_ADJUSTMENT_CM) {
while(diff <= -MOTOR_ADJUSTMENT_CM || diff >= MOTOR_ADJUSTMENT_CM) {
process_sensors();
if(!checkRightWall()) {
break;
}
diff = getIRSensorReadingCM(IR_SENSOR_RF)-getIRSensorReadingCM(IR_SENSOR_RB);
if(diff >= MOTOR_ADJUSTMENT_CM ) {
_stop();
PWM_SetDC(PLATFORM_LEFT_FORWARD ,PLATFORM_PERIOD_PERCENTAGE*MOTOR_ADJUSTMENT_SPEED*MP._left_side._calibrate_speed);
PWM_SetDC(PLATFORM_RIGHT_BACKWARD ,PLATFORM_PERIOD_PERCENTAGE*MOTOR_ADJUSTMENT_SPEED*MP._right_side._calibrate_speed);
} else if ( diff <= -MOTOR_ADJUSTMENT_CM) {
_stop();
PWM_SetDC(PLATFORM_LEFT_BACKWARD ,PLATFORM_PERIOD_PERCENTAGE*MOTOR_ADJUSTMENT_SPEED*MP._left_side._calibrate_speed);
PWM_SetDC(PLATFORM_RIGHT_FORWARD ,PLATFORM_PERIOD_PERCENTAGE*MOTOR_ADJUSTMENT_SPEED*MP._right_side._calibrate_speed);
} else {
break;
}
}
MP.adjust_state |= PLATFORM_ADJUST_DONE;
setOrderCurrentTicks(ticks);
resetPIDIntergrator();
_stop();
setChange(1);
deactivatePID();
}
}
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
files (startup_stm32f40xx.s/startup_stm32f427x.s) before to branch to
application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f4xx.c file
*/
/* USART configuration -----------------------------------------------------*/
USART_Config();
/* SysTick configuration ---------------------------------------------------*/
SysTickConfig();
/* LEDs configuration ------------------------------------------------------*/
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDOn(LED3);//orange
STM_EVAL_LEDOn(LED4);//verte
STM_EVAL_LEDOn(LED5);//rouge
STM_EVAL_LEDOn(LED6);//bleue
//PWM config (motor control)
TIM1_Config();
PWM1_Config(10000);
/* Tamper Button Configuration ---------------------------------------------*/
STM_EVAL_PBInit(BUTTON_USER,BUTTON_MODE_GPIO);
//Set motor speed
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
/* Initialization of the accelerometer -------------------------------------*/
MPU6050_I2C_Init();
MPU6050_Initialize();
if (MPU6050_TestConnection()) {
// connection success
STM_EVAL_LEDOff(LED3);
}else{
STM_EVAL_LEDOff(LED4);
}
//Calibration process
// Use the following global variables and access functions
// to calibrate the acceleration sensor
calibrate_sensors();
zeroError();
//Ready to receive message
/* Enable DMA USART RX Stream */
DMA_Cmd(USARTx_RX_DMA_STREAM,ENABLE);
/* Enable USART DMA RX Requsts */
USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE);
while(1){
//--------------------------------------------------------
//------ Used to configure the speed controller ----------
//--------------------------------------------------------
// press blue button to force motor at SPEED_100
if (STM_EVAL_PBGetState(BUTTON_USER)){
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
Delay(100);
}
//--------------------------------------------------------
//------ Get gyro information ----------
//--------------------------------------------------------
// Read the raw values.
MPU6050_GetRawAccelGyro(AccelGyro);
// Get the time of reading for rotation computations
unsigned long t_now = millis();
STM_EVAL_LEDToggle(LED5);
// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet:
// 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 – (340 * 35) = -12412
//dT = ( (double) AccelGyro[TEMP] + 12412.0) / 340.0;
// Convert gyro values to degrees/sec
gyro_x = (AccelGyro[GYRO_X] - base_x_gyro) / FSSEL;
gyro_y = (AccelGyro[GYRO_Y] - base_y_gyro) / FSSEL;
gyro_z = (AccelGyro[GYRO_Z] - base_z_gyro) / FSSEL;
// Get raw acceleration values
accel_x = AccelGyro[ACC_X];
accel_y = AccelGyro[ACC_Y];
accel_z = AccelGyro[ACC_Z];
// Get angle values from accelerometer
//float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS2DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS2DEGREES;
//float accel_angle_z = 0;
//// Compute the (filtered) gyro angles
//Get the value in second, a tick is every 10ms
dt = (t_now - last_read_time)/100.0;
float gyro_angle_x = gyro_x*dt + lastAngle[X];//get_last_x_angle();
float gyro_angle_y = gyro_y*dt + lastAngle[Y];//(get_last_y_angle();
float gyro_angle_z = gyro_z*dt + lastAngle[Z];//get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + lastGyroAngle[X];//get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + lastGyroAngle[Y];//get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + lastGyroAngle[Z];//get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle – choice of alpha is
// estimated now. Alpha depends on the sampling rate…
float alpha = 0.96;
angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x;
angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y;
angle_z = gyro_angle_z; //Accelerometer doesn’t give z-angle
//printf("%4.2f %4.2f %4.2f\r\n",angle_x,angle_y,angle_z);
//// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
//Stabilisation
// Stable Mode
angl = getAngleFromRC(rcBluetooth[ROLL]);
levelRoll = (getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * PID[LEVELROLL].P;
levelPitch = (getAngleFromRC(rcBluetooth[PITCH]) - angle_y) * PID[LEVELPITCH].P;
// Check if pilot commands are not in hover, don't auto trim
// if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) {
// zeroIntegralError();
// }
// else {
PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((getAngleFromRC(rcBluetooth[PITCH]) + angle_y) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
// }
//motors.setMotorAxisCommand(ROLL,
motor[ROLL] = updatePID(rcBluetooth[ROLL] + levelRoll, gyro_x + 1500, &PID[LEVELGYROROLL],dt) + PID[LEVELROLL].integratedError;//);
//motors.setMotorAxisCommand(PITCH,
motor[PITCH] = updatePID(rcBluetooth[PITCH] + levelPitch, gyro_y + 1500, &PID[LEVELGYROPITCH],dt) + PID[LEVELPITCH].integratedError;//);
getLastSpeedFromMsg();
PWM_SetDC(1, SPEED_0 + SPEED_RANGE*rcSpeed[1] + motor[ROLL] *0.10f); //PE9 | PC6//ON 2ms
//Send data on UART
*(float*)(aTxBuffer) = angle_x;
*(float*)(aTxBuffer+4) = angle_y;
*(float*)(aTxBuffer+8) = angle_z;
*(float*)(aTxBuffer+12) = motor[ROLL];
*(float*)(aTxBuffer+16) = motor[PITCH];
sendTxDMA((uint32_t)aTxBuffer,20);
//Wait a little bit
Delay(3); //30 ms
}
}
void _stop(void) {
PWM_SetDC(1,0);
PWM_SetDC(2,0);
PWM_SetDC(3,0);
PWM_SetDC(4,0);
}
void _turn_right(void) {
_stop();
PWM_SetDC(PLATFORM_LEFT_FORWARD ,PLATFORM_PERIOD_PERCENTAGE*MP._Lspeed*MP._left_side._calibrate_speed);
PWM_SetDC(PLATFORM_RIGHT_BACKWARD ,PLATFORM_PERIOD_PERCENTAGE*MP._Rspeed*MP._right_side._calibrate_speed);
}
