mshuffle_motors.ic

/** File: mshuffle_motors.ic
	Purpose: Subroutines for motor control.
	Parameters: See config.ic
	Author: Gabriel Krell
	Initial commit date: 9/30
**/

void scaleSpeedBy(float val) {
	MOTOR_L_SPEED = (int) (val * (float)MOTOR_L_SPEED);
	MOTOR_R_SPEED = (int) (val * (float)MOTOR_R_SPEED);
}

void forwardFor(float seconds) {
	forward();
	sleep(seconds);
	stop();
	deliberate();
}
void backwardFor(float seconds) {
	backward();
	sleep(seconds);
	stop();
	deliberate();
}
void forward() {
	fdL();
	fdR();
}
void backward() {
	bkL();
	bkR();
}
void leftFor(float seconds) {
	left();
	sleep(seconds);
	stop();
	deliberate();
}
void hardLeftFor(float seconds) {
	hardLeft();
	sleep(seconds);
	stop();
	deliberate();
}
void rightFor(float seconds) {
	right();
	sleep(seconds);
	stop();
	deliberate();
}
void hardRightFor(float seconds) {
	hardRight();
	sleep(seconds);
	stop();
	deliberate();
}


/*Hard turns are full power instead of respecting motor speed
  to avoid rolling over the line too fast when running followLine().
  They do not respect BACKUP_MODE. */
void hardLeft() {
	fd(MOTOR_R);
	bk(MOTOR_L);
}
void hardRight() {
	fd(MOTOR_L);
	bk(MOTOR_R);
}


// use for general navigation, not line following
void left() {
	   fdR();
	   if (BACKUP_MODE) {
		   bkL();
	   } else {
		   off(MOTOR_L);
	   }
}
void right() {
	fdL();
	if (BACKUP_MODE) {
		bkR();
	} else {
		off(MOTOR_R);
	}
}
void fdR() {
	motor(MOTOR_R,MOTOR_R_SPEED);
}
void fdL() {
	motor(MOTOR_L,MOTOR_L_SPEED);
}
void bkL() {
	motor(MOTOR_L,-1*MOTOR_L_SPEED);
}
void bkR() {
	motor(MOTOR_R,-1*MOTOR_R_SPEED);
}
void stop() {
	off(MOTOR_L);
	off(MOTOR_R);
}

void adj_motor() {
	while (1) { 
		MOTOR_L_SPEED = norm_knob();
		MOTOR_R_SPEED = norm_knob();
		printf("%d\n",norm_knob());
		defer();
	}
}

/* "Deliberate mode" briefly pauses the robot in place after movement
	actions, to avoid rolling stops and strange turns.  It can be
	disabled for just the next action with fastNext() or for an
	arbitrary number of actions with fastNextN().	*/
void deliberate() {
	if (DELIBERATE) {
		if (fast_actions_remaining) {
			fast_actions_remaining--;
		} else {
			sleep(.5);
		}
	}
	// gotta go fast
}
void fastNext() {
	fast_actions_remaining++;
}
void fastNextN(int numFastActions) {
	fast_actions_remaining+=numFastActions;
}