/*! \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(); }