/*! \brief Manage the right motor speed
*
* This function manage the right motor speed by changing the MOTOR2
* phases. The changing phases frequency (=> speed) is controled by
* the agenda (throw the function \ref e_set_agenda_cycle(void (*func)(void), int cycle)).
* \param motor_speed from -1000 to 1000 give the motor speed in steps/s,
* positive value to go forward and negative to go backward.
* \sa e_set_agenda_cycle
*/
void e_set_speed_right(int motor_speed) // motor speed in percent
{
// this is to avoid high current drain when the speed is changed very frequently
MOTOR2_PHA = 0;
MOTOR2_PHB = 0;
MOTOR2_PHC = 0;
MOTOR2_PHD = 0;
// speed null
if (motor_speed == 0)
{
right_speed = 0;
e_set_agenda_cycle(run_right_motor, 0);
}
// speed inferior to the minimum value
else if (motor_speed < -1000)
{
right_speed = -1000;
e_set_agenda_cycle(run_right_motor, (int)-10000/right_speed);
}
// speed superior to the maximum value
else if (motor_speed > 1000)
{
right_speed = 1000;
e_set_agenda_cycle(run_right_motor, (int) 10000/right_speed);
}
else
{
right_speed = motor_speed;
// negative speed
if(motor_speed < 0)
e_set_agenda_cycle(run_right_motor, (int)-10000/motor_speed);
// positive speed
else
e_set_agenda_cycle(run_right_motor, (int) 10000/motor_speed);
}
}
/*! \brief Manage the right motor speed
*
* This function manage the right motor speed by changing the MOTOR2
* phases. The changing phases frequency (=> speed) is controled by
* the agenda (throw the function \ref e_set_agenda_cycle(void (*func)(void), int cycle)).
* \param motor_speed from -1000 to 1000 give the motor speed in steps/s,
* positive value to go forward and negative to go backward.
* \sa e_set_agenda_cycle
*/
void e_set_speed_right(int motor_speed) // motor speed in percent
{
// speed null
if (motor_speed == 0)
{
right_speed = 0;
e_set_agenda_cycle(run_right_motor, 0);
MOTOR2_PHA = 0;
MOTOR2_PHB = 0;
MOTOR2_PHC = 0;
MOTOR2_PHD = 0;
}
// speed inferior to the minimum value
else if (motor_speed < -1000)
{
right_speed = -1000;
e_set_agenda_cycle(run_right_motor, (int)-10000/right_speed);
}
// speed superior to the maximum value
else if (motor_speed > 1000)
{
right_speed = 1000;
e_set_agenda_cycle(run_right_motor, (int) 10000/right_speed);
}
else
{
right_speed = motor_speed;
// negative speed
if(motor_speed < 0)
e_set_agenda_cycle(run_right_motor, (int)-10000/motor_speed);
// positive speed
else
e_set_agenda_cycle(run_right_motor, (int) 10000/motor_speed);
}
}
/*! \brief Change the blinking speed
*
* This function use \ref e_set_agenda_cycle(void (*func)(void), int cycle)
* \param cycle the number of cycle we wait before launching \ref e_blink_led(void)"
* \sa e_blink_led, e_set_agenda_cycle
*/
void e_set_blinking_cycle(int cycle)
{
if (cycle>=0)
e_set_agenda_cycle(e_blink_led, cycle);
}
/*! Change left motor phase according to the left_speed sign. */
void run_left_motor(void) // interrupt for motor 1 (of two) = left motor
{
// increment or decrement phase depending on direction
#ifdef POWERSAVE
static int phase_on = 0;
if(phase_on && abs(left_speed) < MAXV) {
MOTOR1_PHA = 0;
MOTOR1_PHB = 0;
MOTOR1_PHC = 0;
MOTOR1_PHD = 0;
phase_on = 0;
e_set_agenda_cycle(run_left_motor, 10000/abs(left_speed) - 10000/TRESHV);
return;
}
#endif
if (left_speed > 0) // inverted for the two motors
{
nbr_steps_left++;
left_motor_phase--;
if (left_motor_phase < 0) left_motor_phase = 3;
}
else
{
nbr_steps_left--;
left_motor_phase++;
if (left_motor_phase > 3) left_motor_phase = 0;
}
// set the phase on the port pins
switch (left_motor_phase)
{
case 0:
{
MOTOR1_PHA = 0;
MOTOR1_PHB = 1;
MOTOR1_PHC = 0;
MOTOR1_PHD = 1;
break;
}
case 1:
{
MOTOR1_PHA = 0;
MOTOR1_PHB = 1;
MOTOR1_PHC = 1;
MOTOR1_PHD = 0;
break;
}
case 2:
{
MOTOR1_PHA = 1;
MOTOR1_PHB = 0;
MOTOR1_PHC = 1;
MOTOR1_PHD = 0;
break;
}
case 3:
{
MOTOR1_PHA = 1;
MOTOR1_PHB = 0;
MOTOR1_PHC = 0;
MOTOR1_PHD = 1;
break;
}
}
#ifdef POWERSAVE
if(abs(left_speed) < MAXV) {
phase_on = 1;
e_set_agenda_cycle(run_left_motor,10000/TRESHV);
}
#endif
}
