/*! \brief Turn a preset angle in [degrees] clockwise or counter-clockwise
* \param turn_angle The angle in [degrees] which is to be traveled
* CW (-) or CCW (+)
* \param motor_speed The speed at which the robot turns. The speed is always positive.
*/
void e_set_turn(double turn_angle, int motor_speed) // added by bahr
{
INTERRUPT_OFF();
double delta_s;
nbr_pas_left = 0;
nbr_pas_right = 0;
// set open loop control status to active
ol_ctrl_status = 1;
// compute the distance which each wheels has to travel (in opposite direction)
delta_s=((double)WHEEL_DISTANCE*(fabs(turn_angle)*(PI/(double)180.0)))/(double)2.0;
// compute the number of necessary steps for the turn
nbr_pas_ol_ctrl = (long int)(((long int)STEPS_PER_M*delta_s));
// ensure that the motor speed is positive
motor_speed=abs(motor_speed);
// turn clock-wise
if (turn_angle < 0)
{
nbr_pas_ol_ctrl_left = nbr_pas_ol_ctrl;
nbr_pas_ol_ctrl_right = -nbr_pas_ol_ctrl;
e_set_speed_left(motor_speed);
e_set_speed_right(-motor_speed);
}
// turn counter clock-wise
if (turn_angle > 0)
{
nbr_pas_ol_ctrl_left = -nbr_pas_ol_ctrl;
nbr_pas_ol_ctrl_right = nbr_pas_ol_ctrl;
e_set_speed_left(-motor_speed);
e_set_speed_right(motor_speed);
}
// stop
if (turn_angle == 0)
{
ol_ctrl_status = 4;
e_set_speed_left(0);
e_set_speed_right(0);
}
INTERRUPT_ON();
}
/*! \brief Manage the right speed
*
* This function manage the right motor speed by changing the MOTOR2
* phases. The changing phases frequency (=> speed) is controled by
* the timer3.
* \param motor_speed from -1000 to 1000 give the motor speed in steps/s,
* positive value to go forward and negative to go backward.
*/
void e_set_speed_right(int motor_speed) // motor speed in steps/s
{
INTERRUPT_OFF();
if (motor_speed != 0)
{
if (motor_speed > 999)
motor_speed = 999;
if (motor_speed < -999)
motor_speed = -999;
motor_counter_right=abs(10000/motor_speed);
}
else
motor_counter_right=9999;
motor_counter_right_init = motor_counter_right;
right_speed = motor_speed;
INTERRUPT_ON();
}
/*! \brief Travel a preset distance in [mm] forward or backward
* \param set_distance The distance in [mm] which is to be traveled
* forward (+) or backward (-)
* \param motor_speed The speed at which the robot travels. The speed is always positive.
*/
void e_set_distance(long int set_distance, int motor_speed) // added by bahr
{
INTERRUPT_OFF();
nbr_pas_left = 0;
nbr_pas_right = 0;
// set open loop control status to active
ol_ctrl_status = 1;
// computebthe number of steps for each motor
nbr_pas_ol_ctrl_left = (long int)(((long int)STEPS_PER_M*set_distance)/1000);
nbr_pas_ol_ctrl_right = (long int)(((long int)STEPS_PER_M*set_distance)/1000);
// go backwards
if (set_distance < 0)
{
motor_speed=-abs(motor_speed);
}
// go forward
if (set_distance > 0)
{
motor_speed=abs(motor_speed);
}
// stop
if (set_distance == 0)
{
ol_ctrl_status = 4;
motor_speed = 0;
}
// set motor speed
e_set_speed_left(motor_speed);
e_set_speed_right(motor_speed);
INTERRUPT_ON();
}
/*! \brief Set the number of right motor steps
* \param set_steps The number of changed phases that you want set.
*/
void e_set_steps_right(int set_steps)
{
INTERRUPT_OFF();
nbr_pas_right = set_steps;
INTERRUPT_ON();
}
/*! \brief Set the number of right motor steps
* \param set_steps The number of changed phases that you want set.
*/
void e_set_steps_right(long int set_steps) // modified by bahr
{
INTERRUPT_OFF();
nbr_pas_right = set_steps;
INTERRUPT_ON();
}
