bool turn_left(int speed)
{
int portA = ROBOT_WHEEL_LEFT_PORT;
int portB = ROBOT_WHEEL_RIGHT_PORT;
bool keep_turning2;
int forward_distance = command_get_forward_distance_mm();
motors_set_speed(portA, -30);
motors_set_speed(portB, 30+speed);
sleep_ms(500);
forward_distance = command_get_forward_distance_mm();
if ( forward_distance < 200) //Tune this parameter!!!!
{
keep_turning2 = true;
}
else
{
sleep_ms(800); // TURN BIT MORE TO MAKE SURE RIGHT SENSOR MEASURES WALL CORRECTLY
keep_turning2 = false;
}
return keep_turning2;
}
void CWheelHandler::move_until_collision()
{
printf("move_until_collision start\n");
time_t starttime = time(NULL);
time_t currenttime;
int elapsed = 0;
int distance = 0;
int percentage = 0;
int velocity = -40;
int actual_velocity_left = 0;
int actual_velocity_right = 0;
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, velocity);
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, velocity);
do
{
sleep(1);
distance = sensors_get_us_distance_mm(ROBOT_ULTRASONIC_SENSOR_PORT);
percentage = sensors_get_ir_distance(ROBOT_INFRARED_SENSOR_PORT);
actual_velocity_right = motors_get_motor_speed(ROBOT_WHEEL_RIGHT_PORT);
actual_velocity_left = motors_get_motor_speed(ROBOT_WHEEL_LEFT_PORT);
currenttime = time(NULL);
elapsed = static_cast<int>(difftime(currenttime, starttime));
printf("move_until_collision dist=%d, perc=%d, velocity=%d, left=%d, right=%d, elapsed=%d\n", distance, percentage, velocity, actual_velocity_left, actual_velocity_right, elapsed);
} while ((elapsed < 30) and /*((distance<120) or (distance>180)) and*/ ((abs(actual_velocity_left)>0) or (abs(actual_velocity_right)>0)));
motors_stop(ROBOT_WHEEL_RIGHT_PORT);
motors_stop(ROBOT_WHEEL_LEFT_PORT);
sleep(1);
printf("move_until_collision done\n");
}
void move_away_from_wall()
{
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, 0);
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, 50);
sleep_ms(400);
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, 50);
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, 0);
sleep_ms(200);
}
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);
}
}
int main()
{
init();
pause_s(1);
motors_stop();
/* Tests */
/*
//consigneDelta = (long)720*RAPPORT_CONVERSION; // 1 tour autour d'une roue
consigneDelta = 216658; // 1 tour autour d'une roue
rapportVitesse = (((long)720*RAPPORT_CONVERSION) << 16)/consigneDelta; // de 0 (droite) à 1
// >> 16 à l'utilisation
// Si > 1 : asser sur alpha//*/
//send_message(rapportVitesse);
/*
consigneAlpha = 0;
consigneDelta = (long)14000*N/D; // 2 cases
rapportVitesse = 0;//*/
//*
consigneDelta = 0;
consigneAlpha = (long)720*RAPPORT_CONVERSION; // 1 tour
rapportVitesse = 3949;//*/ (consigneDelta = 13061)
erreurAlpha += consigneAlpha;
erreurDelta += consigneDelta;
while(1) {
motors_set_speed(LEFT, Vg);
motors_set_speed(RIGHT, Vd);
// if (consigneDelta)
// while(1)
// {
// while (doitAttendre); // Attend l'interruption du Timer
// doitAttendre = TRUE;
// asser_delta();
// if (!doitAttendre) _RB5 = 0; // Timer trop rapide : allume LED debug
// }
// if (consigneAlpha)
// while(1)
// {
// while (doitAttendre); // Attend l'interruption du Timer
// doitAttendre = TRUE;
// asser_alpha();
// if (!doitAttendre) _RB5 = 0; // Timer trop rapide : allume LED debug
// }
}
return 0;
}
void turn_right(int speed)
{
int portA = ROBOT_WHEEL_LEFT_PORT;
int portB = ROBOT_WHEEL_RIGHT_PORT;
int right_distance2 = command_get_right_distance_mm();
if (right_distance2 > 150)
{
motors_set_speed(portA, speed+15);
motors_set_speed(portB, 15);
sleep_ms(50);
}
}
void AD_motors_set_speed(short speedL, short speedR)
{
short commandeL = speedL;
short commandeR = speedR;
if ( speedL > 0)
{
if (speedL > MAX_SPEED )
{
commandeL = MAX_SPEED;
}
}
else if(speedL < 0)
{
if (-speedL > MAX_SPEED )
{
commandeL = -MAX_SPEED;
}
}
if ( speedR > 0)
{
if (speedR > MAX_SPEED )
{
commandeR = MAX_SPEED;
}
}
else if(speedR < 0)
{
if (-speedR > MAX_SPEED )
{
commandeR = -MAX_SPEED;
}
}
motors_set_speed (commandeL,commandeR);
}
void CThrower::calibrate()
{
printf("Calibrate throw engine start\n");
motors_set_speed(ROBOT_BALL_THROW_PORT, 20);
SBYTE motor_speeds[4];
int sum_motor_speeds = 0;
for (int i=0; i<4; i++) motor_speeds[i] = 1;
int i = 0;
time_t starttime = time(NULL);
bool timeout = false;
do
{
sleep_ms(100);
timeout = difftime(time(NULL), starttime) > 4.0; // Open arms shouldn't take more than 4 seconds, otherwise mechanical problems
motor_speeds[(i++)%4] = motors_get_motor_speed(ROBOT_BALL_THROW_PORT);
printf("calibrate %d %d %d %d (timeout=%d)\n", motor_speeds[0], motor_speeds[1], motor_speeds[2], motor_speeds[3], timeout);
sum_motor_speeds = abs(motor_speeds[0]) + abs(motor_speeds[1]) + abs(motor_speeds[2]) + abs(motor_speeds[3]);
} while ((not timeout) and (sum_motor_speeds > 0));
motors_reset_angle(ROBOT_BALL_THROW_PORT);
motors_stop(ROBOT_BALL_THROW_PORT);
is_down = false;
is_half_down = false;
is_up = false;
printf("Calibrate throw engine done\n");
}
void motor_control_regulate_all(){
int i;
for(i = 0; i < REGULATOR_COUNT; i++){
MotorStruct* motor = &(regulators[i]);
motor_control_regulate(motor);
motor->current_output = motors_set_speed(motor->motor_number, motor->current_output);
}
}
int main() {
long int tickG = 0, tickD = 0;
long int vG=7900, vD=7900;
init();
while (tickG < 10 && tickD < 10)
{
vG += 20;
vD += 20;
tickG = 0;
tickD = 0;
motors_reset_qei();
motors_set_speed(echelle(vG), echelle(vD));
pause_ms(70);
motors_get_qei(&tickG, &tickD);
}
motors_set_speed(0, 0);
AD_minSpeed = (long int)((float)vG*1.1);
asser();
return 0;
}
void CThrower::shoot()
{
printf("Throw engine SHOOT start\n");
down();
printf("SHOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOT");
motors_set_speed(ROBOT_BALL_THROW_PORT, -100);
sleep_ms(500);
motors_stop(ROBOT_BALL_THROW_PORT);
sleep_ms(100);
is_up = false;
is_down = false;
is_half_down = false;
printf("Throw engine SHOOT done\n");
}
bool CCatcher::open()
{
printf("Catch engine OPEN start\n");
bool is_ok = true;
if (not is_open)
{
int try_count = 0;
is_ok = false;
while ((not is_ok) and (try_count++ < 2))
{
motors_set_speed(ROBOT_BALL_CATCH_PORT, direction * 30);
SBYTE motor_speeds[4];
int sum_motor_speeds = 0;
for (int i=0; i<4; i++) motor_speeds[i] = 1;
int i = 0;
int current_angle = motors_get_angle(ROBOT_BALL_CATCH_PORT);
time_t starttime = time(NULL);
bool timeout = false;
do
{
sleep_ms(100);
timeout = difftime(time(NULL), starttime) > 3.0; // Open arms shouldn't take more than 3 seconds, otherwise mechanical problems
motor_speeds[(i++)%4] = motors_get_motor_speed(ROBOT_BALL_CATCH_PORT);
printf("CCatcher.open %d %d %d %d (timeout=%d)\n", motor_speeds[0], motor_speeds[1], motor_speeds[2], motor_speeds[3], timeout);
sum_motor_speeds = abs(motor_speeds[0]) + abs(motor_speeds[1]) + abs(motor_speeds[2]) + abs(motor_speeds[3]);
} while ((not timeout) and (sum_motor_speeds > 0));
int reached_angle = motors_get_angle(ROBOT_BALL_CATCH_PORT);
motors_reset_angle(ROBOT_BALL_CATCH_PORT);
motors_stop(ROBOT_BALL_CATCH_PORT);
// is_ok also true when started with half open arms
int rotation = abs(current_angle - reached_angle);
is_ok = (timeout) ? false : ((is_closed) ? (rotation > 170) : (rotation > 10));
if (not is_ok and (try_count == 1))
{
printf("Trying to open arms with motor in reverse direction!\n");
direction *= -1; // Try once more with motor in another direction
sleep(1);
}
}
}
is_open = true;
is_closed = false;
printf("Catch engine OPEN result %d\n", is_ok);
return is_ok;
}
void maze_execute(bool* keep_running)
{
// 5000ms and speed 50km/h
move_forward(5000, 50);
while (*keep_running)
{
int forward_distance = command_get_forward_distance_mm();
int right_distance = command_get_right_distance_mm();
// Do we need to START turning left?
if (not must_turn_left)
{
if ((forward_distance < Desired_dist_front) && (right_distance < 3*Desired_dist_right))
{
must_turn_left = true;
}
}
// Turn left
if (must_turn_left)
{
bool Var_turning = turn_left(Speed_turn_left);
must_turn_left = Var_turning;
lasterror = 0;
}
// Turn right
if ((right_distance > 3*Desired_dist_right) && (right_distance < 2200) && (not must_turn_left))
{
turn_right(Speed_turn_right);
lasterror = 0;
}
// Move away from wall
if ((right_distance >= 2200) && (not must_turn_left))
{
move_away_from_wall();
lasterror = 0;
}
// Follow wall
if ((not must_turn_left) && (forward_distance >= Desired_dist_front) && (right_distance < 3*Desired_dist_right)) // Follow wall
{
// Control action
error = right_distance - Desired_dist_right;
derivative = error - lasterror;
Turn = 2*error + 2*derivative;
Motor_speed_left = Motor_speed_forward + Turn;
Motor_speed_right = Motor_speed_forward - Turn;
// Saturation motor speeds
if ( Motor_speed_left > 100) { Motor_speed_left = 100; }
if ( Motor_speed_left < -100) { Motor_speed_left = -100; }
if ( Motor_speed_right > 100) { Motor_speed_right = 100; }
if ( Motor_speed_right < -100) { Motor_speed_right = -100; }
// Set motor
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, Motor_speed_left);
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, Motor_speed_right);
lasterror = error;
sleep_ms(25); // Prevent busy loop
}
}
printf("maze_execute stopping...\n");
command_move_stop();
}
void asser() {
long int erreurs_delta[4] = {0,0,0,0};
long int erreurs_alpha[4] = {0,0,0,0};
int i ;
long int debuga = 0;
///////////////////////////
motors_get_qei(&AD_distG, &AD_distD);
/////////////////////////////////
while(1)
{
if( reset)
{
reset = 0;
for( i=0; i<4;i++)
{
erreurs_delta[i] = 0;
erreurs_alpha[i] = 0;
}
AD_delta =0;
AD_alpha =0;
AD_commandePrec = 0;
AD_distG = 0;
AD_distD = 0;
motors_reset_qei();
}
#ifndef SIMU
while (doitAttendre); // Attend l'interruption du timer pour une fréquence régulière
doitAttendre = TRUE;
#endif
AD_commandePrec = AD_commandeDelta;
///////////////////////
motors_get_qei(&AD_distG, &AD_distD);
AD_alpha = (AD_distD - AD_distG)/(2.0*1831); //1900 = Distance inter roues codeuses en tick/2
AD_delta = (AD_distD + AD_distG)/2;
//alpha
AD_ecart = AD_consAlpha - AD_alpha;
maj_erreurs(erreurs_alpha, AD_ecart);
AD_commandeAlpha = AD_ecart*AD_kP_alpha - deriv_erreurs(erreurs_alpha)*AD_kD_alpha;
if(AD_abs(AD_ecart) < AD_presAlpha)
{
AD_commandeAlpha=0;
}
//delta
AD_ecart = AD_consDelta - AD_delta;
maj_erreurs(erreurs_delta, AD_ecart);
debuga = deriv_erreurs(erreurs_delta);
AD_commandeDelta = AD_ecart*AD_kP_delta - deriv_erreurs(erreurs_delta)*AD_kD_delta;
//Rampe de vitesse
AD_commandeAlpha = rampe_alpha(AD_commandeAlpha);
AD_commandeDelta = rampe_delta(AD_commandeDelta, AD_commandePrec);
//Precision en delta
if( AD_abs(AD_ecart) < AD_presDelta )
{
AD_commandeDelta = 0;
}
motors_set_speed (AD_commandeDelta - AD_commandeAlpha, AD_commandeDelta + AD_commandeAlpha);
//////////////////////////////////
if (!doitAttendre) allumer_del(); // Timer trop rapide : allume LED debug
}
}
int motor_control_set_motor_speed(MotorStruct* motor, int value){
return motors_set_speed(motor->motor_number, value + motor->op_point);
}
void move_forward(int duration_ms, int speed)
{
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, speed);
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, speed);
sleep_ms(duration_ms);
}
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);
}
}
