void asser_tourner(long consigne){
long delta = 0;
long alpha = 0;
PID_init(&pid_alpha,100,0,0); // nb ki divisé par 10 pour système stable
PID_init(&pid_delta,2000,0,0);
long max_consigne = 400; // Consigne maximale donnée au pid sans saturation (10 cm)
long max_pid = 40000*2;
while(((alpha < consigne && consigne > 0) || ( alpha > consigne && consigne < 0)) && !stop_asser){
/** Actualisation position actuelle (l, r) **/
l = -motors_get_qei(LEFT);//*4; //recupere les données des codeurs, repesentant dd
motors_reset_qei(LEFT); //remise a zero des codeurs
r = -motors_get_qei(RIGHT);//*4+1;
motors_reset_qei(RIGHT);
/** Changement variable (l, r) -> (delta, alpha) **/
delta += (r + l)/2; // diviser par 2 ?
alpha += (r - l)/2; // valeur négatives ?
/** PID asservissement de position **/
if( consigne - alpha>max_consigne ){
PID_run(&pid_alpha, (max_consigne));
}
else{
if(consigne - alpha<-max_consigne){
PID_run(&pid_alpha, -(max_consigne));
}
else{ PID_run(&pid_alpha, (consigne - alpha));}
}
PID_run(&pid_delta, -delta);
commande_alpha = pid_alpha.out;
commande_delta = pid_delta.out;
/** Limitation A sur alpha **/
//*/
if ((commande_alpha-commande_alpha_old)>LIMITE_A_SUR_ALPHA) commande_alpha = commande_alpha_old+LIMITE_A;
if ((commande_alpha-commande_alpha_old)<(-LIMITE_A_SUR_ALPHA)) commande_alpha = commande_alpha_old-LIMITE_A;
commande_alpha_old = commande_alpha;
//*/
/** Changement de variable (commande_delta, commande_alpha) -> (commande_r, commande_l) **/
commande_r = ( commande_delta + commande_alpha );
commande_l = ( commande_delta - commande_alpha );
/** envoi au moteur **/
//motors_set_speed envoi commande_r sous forme de rapport cyclique (en divisant par 200)
motors_set_speed(RIGHT, commande_r*(long)VITESSE/max_pid); // avec
motors_set_speed(LEFT, commande_l*(long)VITESSE/max_pid);
}
if(consigne != 0){
consigne_alpha = 0;
motors_stop();
init_parametre();
send_message(DONE);
}
}
void init_parametre(){
PID_init(&pid_alpha,0,0,0); // nb ki divisé par 10 pour système stable
PID_init(&pid_delta,0,0,0);
motors_reset_qei(LEFT);
motors_reset_qei(RIGHT);
alpha = 0; // Position (alpha, delta)
delta = 0;
consigne_alpha = 0; // Consigne (alpha, delta)
consigne_delta = 0;
commande_alpha = 0; // Commande (alpha, delta)
commande_alpha_old = 0;
commande_delta = 0;
commande_delta_old = 0;
commande_l = 0; // Commande converties sur chaque roue
commande_r = 0;
l = 0; // Vitesse de chaque roue
r = 0;
stop_asser = 1;
}
//*****************************************************************************
// PID control : 200Hz
//*****************************************************************************
void Init_PID(){
// Rate pid
PID_init(&_r_rate_PID, r_rate_Kp, r_rate_Ki, r_rate_Kd, PID_Direction_Direct);
PID_init(&_p_rate_PID, p_rate_Kp, p_rate_Ki, p_rate_Kd, PID_Direction_Direct);
PID_init(&_y_rate_PID, y_rate_Kp, y_rate_Ki, y_rate_Kd, PID_Direction_Direct);
PID_SetSampleTime(&_r_rate_PID, 5); // 0.005s
PID_SetSampleTime(&_p_rate_PID, 5);
PID_SetSampleTime(&_y_rate_PID, 5);
PID_SetMode(&_r_rate_PID, PID_Mode_Automatic);
PID_SetMode(&_p_rate_PID, PID_Mode_Automatic);
PID_SetMode(&_y_rate_PID, PID_Mode_Automatic);
PID_SetOutputLimits(&_r_rate_PID,-20.0,20.0);
PID_SetOutputLimits(&_p_rate_PID,-20.0,20.0);
PID_SetOutputLimits(&_y_rate_PID,-20.0,20.0);
_prev_angle_error = 0;
}
int main(void)
{
ADC_Config();
PID_init();
PWM_init(405);
sei();
while(1)
{
//TODO:: Please write your application code
}
cli();
}
void Reflow_Init(void) {
// PID_init(&PID,16,0.1,2,PID_Direction_Direct);
PID_init(&PID,17,0.11,2,PID_Direction_Direct);
EEPROM_Read((uint8_t*)ee1.temperatures, 2, 96);
ByteswapTempProfile(ee1.temperatures);
EEPROM_Read((uint8_t*)ee2.temperatures, 128+2, 96);
ByteswapTempProfile(ee2.temperatures);
intsetpoint = 30;
PID.mySetpoint = 30.0f; // Default setpoint
PID_SetOutputLimits(&PID, 0,255+248);
PID_SetMode(&PID, PID_Mode_Manual);
PID.myOutput = 248; // Between fan and heat
PID_SetMode(&PID, PID_Mode_Automatic);
RTC_Zero();
}
int main(void)
{
PWM_init();
CF_init(&alpha, &beta);
PID_init(&pid_x, &pid_y);
Flags_init();
Timer1_init();
Twi_init();
Usart_init();
deviceInit_adc();
deviceInit_rc();
if(!deviceInit() || !Flags_getBit(flags_deviceAdcInitialised) || !Flags_getBit(flags_deviceRcInitialised))
{
_exit();
}
// todo: workaround for slave initialization
deviceInitialize(deviceID_slave, _true);
// todo: workaround for gps initialization
deviceInitialize(deviceID_gps, _true);
//enable interrupts
_enableInterrupts();
while(1);
}
int main() {
usartInit(57600);
usartEnableReceiver();
usartEnableTransmitter();
usartStdio();
setBit(DDRB, M1_DIR);
setBit(DDRD, M1_PWM);
setBit(DDRD, M2_DIR);
setBit(DDRD, M2_PWM);
clrBit(PORTB, M1_DIR);
clrBit(PORTD, M1_PWM);
clrBit(PORTD, M2_PWM);
clrBit(PORTD, M2_DIR);
timer0FastPWMMaxMode();
timer0ClockPrescaller64();
timer0OC0ANonInvertedMode();
timer0OC0BNonInvertedMode();
timer0SetCompareAValue(0);
timer0SetCompareBValue(0);
timer0DeactivateCompareAInterrupt();
timer0DeactivateCompareBInterrupt();
timer2ClockPrescaller256();
timer2NormalMode();
timer2OC2AOff();
timer2OC2BOff();
timer2ActivateOverflowInterrupt();
i2c_init();
adxl345_init();
l3g4200d_init();
l3g4200d_setoffset(0.11, -1.71, -0.46);
//l3g4200d_settemperatureref();
PID_init(&PID, 15,0.2,1, 0);
PID_SetMode(&PID, 1);
PID_SetOutputLimits(&PID, -255, 255);
PID.mySetpoint = 0;
motor1(0);
motor2(0);
// printf("\e[1;1H\e[2J");
// printf("STARTED!\r\n");
// _delay_ms(200);
// motor1(120);
// printf("Motor1 = 120\r\n");
// _delay_ms(500);
// motor1(-120);
// printf("Motor1 = -120\r\n");
// _delay_ms(500);
// motor1(120);
// printf("Motor1 = 120\r\n");
// _delay_ms(500);
// motor1(0);
// printf("Motor1 = 0\r\n");
sei();
while(1);
return 0;
}
void Return(Let *let)
{
static uint8 step = 0;
static uint16 count = 0;
static uint16 limit = 1;
Sensor_LED_Write(0);
if(let->place < 3)
{
if(step == 0)
{
Motor_Right(-3500);
Motor_Left(-3500);
//limit = let->count + let->place*40;
limit = let->count;
} else if(step == 1)
{
if(let->count_r > 0)
{
Motor_Right(3000);//反時計
Motor_Left(-3000);
limit = (int)(let->count_r);
}
else if(let->count_r <= 0)
{
Motor_Right(-3000);
Motor_Left(3000);
limit = (int)(-let->count_r);
}
} else if(step == 2)
{
if(let->color == MISS)
{
Motor_Right(0);
Motor_Left(0);
step = 0;
count = 0;
let->count = 0;
let->count_r = 0;
let->mode = MODE_SEEK;
return;
}
if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
{
step++;
Motor_Right(0);
Motor_Left(0);
count = 0;
return;
}
else
{
Motor_Right(-7000);
Motor_Left(-7000);
limit = 0;
}
} else if(step == 3)
{
Motor_Right(7000);
Motor_Left(7000);
limit = 35;
}
else if(step == 4)
{
Motor_Right(0);
Motor_Left(7000);
limit = 60;
} else if(step == 5)
{
Motor_Right(7000);
Motor_Left(7000);
let->area = 3;
PID_init(let);
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
limit = 35;
} else if(step == 6)
{
Line_Trace(let,MODE_BACKWARD);
limit = 0;
}
}
else if(let->place >= 3)
{
if(step == 0)
{
Motor_Right(-3500);
Motor_Left(-3500);
limit = 0;
if(UART_Line_Sensor_GetRxBufferSize())
{
let->slave.Trans = (uint8)UART_Line_Sensor_GetChar();
}
if(let->slave.status.e)
{
step = 1;
count = 0;
Debug_LED_Write(1);
return;
}
} else if(step == 1)
{
if(let->count_r > 0)
{
Motor_Right(3000);//反時計
Motor_Left(-3000);
limit = (int)(let->count_r + 20);
}
else if(let->count_r <= 0)
{
Motor_Right(-3000);
Motor_Left(3000);
limit = (int)(-let->count_r);
}
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
} else if(step == 2)
{
if(let->color == MISS)
{
Motor_Right(0);
Motor_Left(0);
step = 0;
count = 0;
let->count = 0;
let->count_r = 0;
let->mode = MODE_SEEK;
return;
}
if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
{
step++;
Motor_Right(0);
Motor_Left(0);
count = 0;
return;
}
else
{
Motor_Right(-7000);
Motor_Left(-7000);
limit = 0;
}
} else if(step == 3)
{
Motor_Right(7000);
Motor_Left(7000);
limit = 35;
}
else if(step == 4)
{
Motor_Right(0);
Motor_Left(7000);
limit = 60;
} else if(step == 5)
{
Motor_Right(7000);
Motor_Left(7000);
let->area = 3;
PID_init(let);
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
limit = 35;
} else if(step == 6)
{
Line_Trace(let,MODE_BACKWARD);
limit = 0;
}
}
if(let->color == RED)
{
if(let->area == 0)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
else if(let->color == YELLO)
{
if(let->area == 1)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
else if(let->color == BLUE)
{
if(let->area == 2)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
count++;
if(limit == count)
{
count = 0;
step++;
return;
}
}
/*=====================================================================================================*/
void PID_Init_Start(void)
{
PID_init(&PID_Roll, 0.002, 0, 0);
PID_init(&PID_Pitch, 0.001, 0, 0);
PID_init(&PID_Yaw, 0.001, 0, 0);
}
void asser_avancer(long consigne) {
long delta = 0;
long alpha = 0;
// kp = 36000 pour limite oscillation
//PID_init(&pid_alpha,16800,170,0); // nb ki divisé par 10 pour système stable
PID_init(&pid_alpha,1000,10,0); // nb ki divisé par 10 pour système stable
//PID_init(&pid_alpha,100,0,0);
PID_init(&pid_delta,100,0,0);
long max_consigne = 13453; // Consigne maximale donnée au pid sans saturation (10 cm)
long max_pid = 1345300;
consigne = consigne*8;
long consigne_temp = 0;//max_pid* V_MAX_DEMARRAGE/VITESSE; pour partir plus vite, ne pas commencer la rampe à vitesse 0
while(((delta < consigne && consigne > 0) || ( delta> consigne && consigne < 0)) && !stop_asser){
// RAMPE
/*
if (consigne > 0) {
if (consigne_temp <= 0) consigne_temp = 0;
if (consigne_temp >= consigne) consigne_temp = consigne;
else consigne_temp += 13453 /250;
} else {
if (consigne_temp > -max_pid * V_MAX_DEMARRAGE/VITESSE) consigne_temp = -max_pid * V_MAX_DEMARRAGE/VITESSE;
if (consigne_temp < consigne_delta) consigne_temp = consigne_delta;
else consigne_temp -= 10;
}
//*/
consigne_temp = consigne;
/** Actualisation position actuelle (l, r) **/
l = -motors_get_qei(LEFT)*8; //recupere les données des codeurs, repesentant dd
r = -motors_get_qei(RIGHT)*8+3;
motors_reset_qei(LEFT);
motors_reset_qei(RIGHT);
/** Changement variable (l, r) -> (delta, alpha) **/
delta += (r + l)/2; // diviser par 2 ?
alpha += (r - l)/2; // valeur négatives ?
/** PID asservissement de position **/
PID_run(&pid_alpha, -alpha);
if( consigne_temp - delta>max_consigne ){
PID_run(&pid_delta, (max_consigne));
}
else{
if(consigne_temp - delta<-max_consigne){
PID_run(&pid_delta, -(max_consigne));
}
else{ PID_run(&pid_delta, (consigne_temp - delta));} // s'arrete brutalement
}//*/
commande_alpha = pid_alpha.out;
commande_delta = pid_delta.out;
/** Limitation A sur delta **/
if ((commande_delta-commande_delta_old ) > LIMITE_A ) commande_delta = commande_delta_old+LIMITE_A;
if ((commande_delta-commande_delta_old )<(-LIMITE_A)) commande_delta = commande_delta_old-LIMITE_A;
commande_delta_old = commande_delta;
/** Changement de variable (commande_delta, commande_alpha) -> (commande_r, commande_l) **/
commande_r = ( commande_delta + commande_alpha );
commande_l = ( commande_delta - commande_alpha );
/** envoi au moteur **/
//motors_set_speed envoi commande_r sous forme de rapport cyclique (en divisant par 200)
motors_set_speed(RIGHT, commande_r*(long)VITESSE/max_pid); // avec
motors_set_speed(LEFT, commande_l*(long)VITESSE/max_pid);
}
if(consigne != 0){
consigne_delta = 0;
motors_stop();
init_parametre();
send_message(DONE);
}
}
