rp6robotbaselib.c

RP6机器人范例程序。包括移动

/*****************************************************************************/
// Motor speed control

#define MC_LEFT_IMAX 420
#define MC_LEFT_IMIN -420
#define MC_RIGHT_IMAX 420
#define MC_RIGHT_IMIN -420

int16_t right_i; 
int16_t left_i; 
int16_t mleft_power;
int16_t mright_power;
uint8_t mleft_ptmp;
uint8_t mright_ptmp;

uint16_t mleft_des_speed;
uint16_t mright_des_speed;
uint16_t mleft_des_speed_tmp;
uint16_t mright_des_speed_tmp;

uint8_t mleft_des_dir = FWD;
uint8_t mright_des_dir = FWD;
uint8_t mleft_dir = FWD;
uint8_t mright_dir = FWD;

uint8_t overcurrent_timeout;
uint8_t overcurrent_timer;
uint8_t overcurrent_errors;
uint8_t overcurrent_error_clear;

uint8_t motion_status_tmp;

uint16_t distanceToMove_L;
uint16_t distanceToMove_R;
uint16_t preDecelerate_L;
uint16_t preDecelerate_R;
uint16_t preStop_L;
uint16_t preStop_R;

volatile uint8_t motor_control;


// -------------------------------
// MotionControl state changed handler:

void MOTIONCONTROL_stateChanged_DUMMY(void){}
static void (*MOTIONCONTROL_stateChangedHandler)(void) = MOTIONCONTROL_stateChanged_DUMMY;
/**
 * Use this function to set the Motion Control state change handler. 
 * 
 */
void MOTIONCONTROL_setStateChangedHandler(void (*motionControlHandler)(void)) 
{
	MOTIONCONTROL_stateChangedHandler = motionControlHandler;
}
// -------------------------------


// Outcomment this:
#define ENABLE_OC_ERROR_MESSAGE
// to disable the error messages (s. below) in order to save some program space!
// The motionControl function will still check for any errors and stop all operations
// if neccessary.
// This option just disables the error message for overcurrent or 
// defect encoder/motor events.

// Error type:
#define ENCODER_MALFUNCTION_LEFT 1
#define ENCODER_MALFUNCTION_RIGHT 2
#define OVERCURRENT 3

/**
 * If there are any errors like overcurrent or encoder malfunction, this function is 
 * called from task_motionControl (s. below) and outputs an error message and then stops
 * all operations to save the robot from damages that may occur if it continues
 * to drive around. 
 */
void emergencyShutdown(uint8_t why)
{
	cli();
	IRCOMM_OFF();
	setACSPwrOff();
	mleft_power = 0;
	mright_power = 0;
	left_i = 0;
	right_i = 0;
	mleft_ptmp = 0;
	mright_ptmp = 0;
	OCR1AL = 0;
	OCR1BL = 0;
	TCCR1A = 0;
	powerOFF();
#ifdef ENABLE_OC_ERROR_MESSAGE
	writeString_P("\n\n##### EMERGENCY SHUTDOWN #####\n");
	writeString_P("##### ALL OPERATIONS STOPPED TO PREVENT ANY DAMAGE! #####\n");
	if(why == ENCODER_MALFUNCTION_LEFT || why == ENCODER_MALFUNCTION_RIGHT) {
		
		writeString_P("\n### ENCODER (OR MOTOR) MALFUNCTION! ###\n");
		writeString_P("Affected channel:"); 	
		if(why == ENCODER_MALFUNCTION_LEFT)
			writeString_P("LEFT");
		else
			writeString_P("RIGHT");
		writeString_P("!\n\n(s. task_motorControl() function in RP6Lib!)\n");
		writeString_P("You need to check Encoder/Motor assembly (or your software).\n\n");
	}
	else if(why == OVERCURRENT)
	{
		writeString_P("\n\n### MOTOR OVERCURRENT ###\n");
		writeString_P("\n\n(s. task_motorControl() function in RP6Lib!)\n");
		writeString_P("You need to check Motor assembly (or your software).\n\n");
	}
	writeString_P("The Robot needs to be resetted now.\n\n");
#endif
	while(true) // Rest In Peace
	{
		setLEDs(0b110110);
		uint8_t dly;
		for(dly = 10; dly; dly--)
			delayCycles(32768);
		setLEDs(0b000000);
		for(dly = 10; dly; dly--)
			delayCycles(65535);
	}
}

/**
 * Automatic motor speed regulation, direction control, move specific distances and
 * overcurrent+motor+encoder malfunction detection.
 * You must call this function frequently out of the main loop if you
 * want to use the motors! (this call is included in the task_RP6System function!)
 * 
 * This is the most important motor control function!
 *
 * Integral regulation only - works OK but could be improved by 
 * implementing PI or PID regulation.
 * 
 * The speed increase is limited by Soft-PWM adjustment (can also be used to make the
 * acceleration of the motors a lot slower just by software) in the Timer0 interrupt.
 * This intends to increase motor and gears life - along with slow turning direction
 * change! Fast changes at high speeds can wear out the gears over time. 
 *
 * You need to consider this Soft-PWM when changing/tuning this function!
 *
 */
void task_motionControl(void)
{
	// Automatic motor overcurrent shutdown:
	if(overcurrent_timer >= 50) { // every 5ms
		overcurrent_timer = 0;
		if(!overcurrent_timeout) {
			if((adcMotorCurrentLeft > 770) || (adcMotorCurrentRight > 770)) {
				overcurrent_errors++;
				overcurrent_timeout = 10; 
				mleft_power = 0;
				mright_power = 0;				
				left_i = 0;
				right_i = 0;
				motion_status.overcurrent = true;
				return;
			}
			else
				motion_status.overcurrent = false;
			
			// Emergency shutdown if there are too many (default: 3) overcurrent
			// events within ~20 seconds (100 * 200ms).
			if(overcurrent_error_clear > 100) {
				overcurrent_errors = 0;
				overcurrent_error_clear = 0;
			}
			else if(overcurrent_errors > 2)
				emergencyShutdown(OVERCURRENT);
		}
		
		// Detect if one of the encoders or motors does not work properly and stop 
		// all operations immediately if this is the case! 
		if((adcMotorCurrentLeft < 150) && (mleft_speed == 0) 
		  && (mleft_des_speed != 0) &&  (mleft_ptmp > 150))
			emergencyShutdown(ENCODER_MALFUNCTION_LEFT);
		if((adcMotorCurrentRight < 150) && (mright_speed == 0) 
		  && (mright_des_speed != 0) && (mright_ptmp > 150))
			emergencyShutdown(ENCODER_MALFUNCTION_RIGHT);
	}
	
	// Motor Control
	if(motor_control) { // Everytime after the speed has been measured. (default: 200ms)
		if(!overcurrent_timeout) { // No overcurrent timeout? (default is to wait 2 seconds before new try)
			if(overcurrent_errors) // Overcurrent errors?
				overcurrent_error_clear++; // Yes, Timeout to clear all error events.
			else
				overcurrent_error_clear=0; // No, we set the timeout to zero.
				
			// Move Distance left:
			if(motion_status.move_R) {
				if(mleft_dist >= preStop_R) { // Stop a bit before the desired distance for ..
					mleft_des_speed = 0;      // ... better accurancy.
					left_i = 0;
					mleft_power = 0;		
					motion_status.move_R = false;
				}
				else if(mleft_dist >= preDecelerate_R) { // Start to decelerate?
					mleft_des_speed /= 2;
					if(mleft_des_speed < 22) mleft_des_speed = 22;
				}	
			}
			
			// Move Distance right:
			if(motion_status.move_L) {
				if(mright_dist >= preStop_L) { // Stop a bit before the desired distance for ..
					mright_des_speed = 0;      // ... better accurancy.
					right_i = 0;
					mright_power = 0;
					motion_status.move_L = false;
				}
				else if(mright_dist >= preDecelerate_L) { // Start to decelerate?
					mright_des_speed /= 2;
					if(mright_des_speed < 22) mright_des_speed = 22;
				}	
			}
			
			// Change direction -- slowdown and stop before changing direction  
			// to improve gears and motors lifetime:
			if(mleft_des_dir != mleft_dir || mright_des_dir != mright_dir) {
				if(mleft_des_speed || mright_des_speed) {
					mleft_des_speed_tmp = mleft_des_speed; // store current speed
					mright_des_speed_tmp = mright_des_speed; 
					mleft_des_speed = 0;			
					mright_des_speed = 0;
					left_i /= 2;
					right_i /= 2;
				}
				if(mright_speed <= 25 && mleft_speed <= 25 
				  && (!mleft_des_speed) && (!mright_des_speed)) {
					mright_power=0; // Soft PWM adjust to 0
					mleft_power=0;
				}
				if(!TCCR1A) // Is the PWM module turned off?
				{			// Yes, change direction and restore old speed:
					setMotorDir(mleft_des_dir,mright_des_dir);
					mleft_des_speed = mleft_des_speed_tmp;
					mright_des_speed = mright_des_speed_tmp;
					left_i = 0;
					right_i = 0;
				}
			}

			// Left motor speed control:
			int16_t error_left = mleft_des_speed - mleft_speed;
			left_i = left_i + error_left;
			if(left_i > MC_LEFT_IMAX) left_i = MC_LEFT_IMAX;
			if(left_i < MC_LEFT_IMIN) left_i = MC_LEFT_IMIN;
			if(mleft_speed == 0 && mleft_des_speed == 0)
				left_i = 0;
			mleft_power = left_i / 2; 
			if(mleft_power > 210) mleft_power = 210;
			if(mleft_power < 0) mleft_power = 0;
			
			// Right motor speed control:
			int16_t error_right = mright_des_speed - mright_speed;
			right_i = right_i + error_right;
			if(right_i > MC_RIGHT_IMAX) right_i = MC_RIGHT_IMAX;
			if(right_i < MC_RIGHT_IMIN) right_i = MC_RIGHT_IMIN;
			if(mright_speed == 0 && mright_des_speed == 0)
				right_i = 0;
			mright_power = right_i / 2;
			if(mright_power > 210) mright_power = 210;
			if(mright_power < 0) mright_power = 0;
		}
		else
			overcurrent_timeout--;
		motor_control = false;
	}
	
	// Call event handlers if necessary:
	if(motion_status_tmp != motion_status.byte)
	{
		motion_status_tmp = motion_status.byte;
		MOTIONCONTROL_stateChangedHandler();
	}
}

/**
 * This function sets the desired speed value. 
 * The rest is done automatically. The speed is regulated to this speed value 
 * independent of Battery voltage, surface, weight of the robot and other things. 
 *
 * You need to call task_motorSpeedControl();  frequently out of the main loop!
 * otherwise this function will not work!
 * Or use task_RP6System which includes this call. 
 *
 * The function limits maximum speed to 200! This has been done because the maximum
 * possible speed gets lower over time due to battery discharging (--> voltage drop).
 * And the gears and motors will live longer if you don't stress them that much.
 *
 * Also 200 leaves a bit room to the maximum possible PWM value when you 
 * put additional load onto the Robot or drive up a ramp etc.  
 *
 */
void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right)
{
	if(desired_speed_left > 200) desired_speed_left = 200; 
	if(desired_speed_right > 200) desired_speed_right = 200;
	mleft_des_speed = desired_speed_left;
	mright_des_speed = desired_speed_right;
}

uint8_t drive_dir = FWD;

/**
 * This functions sets the desired turning direction of the two motors. 
 * The Robot first slows down, stops, changes direction and then it accelerates again
 * to the previours speed (if the robot was driving... ).  
 * This is done to increase motors and gears lifetime and to avoid hard cut changes.
 *
 */
void changeDirection(uint8_t dir)
{
	drive_dir = dir;
	mleft_des_dir = (dir == BWD || dir == LEFT);
	mright_des_dir = (dir == BWD || dir == RIGHT);
}

/**
 * You can use this function to check if there is any movement going on or if
 * every operation like moving a specific distance or rotating has been finished. 
 */
inline uint8_t isMovementComplete(void)
{
	return !(motion_status.move_L || motion_status.move_R);
}

/**
 * If there are any operations like moving a specific distance or rotating or
 * any motion at all, you can stop the robot with this function. 
 * This can be used for example if the Bumpers detected and obstacle during
 * movement...
 */
void stop(void)
{
	mleft_des_speed = 0;
	mright_des_speed = 0;
	left_i = 0;
	right_i = 0;
	motion_status.move_L = false;
	motion_status.move_R = false;
	motion_status_tmp = motion_status.byte;
	MOTIONCONTROL_stateChangedHandler();
}


/**
 * The robot can use the encoders to measure the distance it has driven. 
 * This function allows you to tell the robot to drive a specific distance 
 * at a given speed and then stop. 
 * Of course this only works if you calibrated the Encoders before! 
 * Also read the RP6 Manual for additional details and have a look at the
 * movement example programs where you also find a detail description of
 * the parameters along with usage examples! 
 *
 * The function sets some parameters that make the robot decelerate a bit
 * before reaching the target distance. This makes it more accurate.
 * If you need faster reaction rather than precision then you should implement 
 * your own routine and set the distance a bit lower... 
 *
 */
void move(uint8_t desired_speed, uint8_t dir, uint16_t distance, uint8_t blocking)
{
	motion_status.move_L = true;
	motion_status.move_R = true;
	preDecelerate_L = 0;
	preDecelerate_R = 0;
	if(desired_speed > 22) {
		preDecelerate_L = distance - (20+(desired_speed*2));
		preDecelerate_R = preDecelerate_L;
	}
	preStop_L = distance - 2;
	preStop_R = preStop_L;
	if(distance < 40) {
		distance = 40; 
		preStop_L = 20;
		preStop_R = preStop_L;
		preDecelerate_L = 10;
		preDecelerate_R = preDecelerate_L;
	}
	if(distance < 400 && desired_speed > 40) {
		desired_speed = 40; 
		preDecelerate_L = distance - (distance/4);
		preDecelerate_R = preDecelerate_L;
	}
    mleft_dist = 0; 
	mright_dist = 0;
	moveAtSpeed(desired_speed,desired_speed);
	changeDirection(dir);
	
	distanceToMove_L = distance;
	distanceToMove_R = distance;

	motion_status_tmp = motion_status.byte;
	
	motion_status_tmp = motion_status.byte;
	MOTIONCONTROL_stateChangedHandler();
	
	if(blocking)
		while(!isMovementComplete())
			task_RP6System();
}

/**
 * You can let the Robot rotate specific angles with this function. 
 * ATTENTION: THIS IS NOT PRECISE!!! (+-10