Home | Design | Code | Tips

Code

This is the code for our robot. We used a pid loop for driving. Other than that, we wrote simple actions and put them together in various routines for the robot to do.

Overall Strategy

Our strategy involved first using 4 IR sensors to detect position. Once we figured out where we were, our robot would orient itself toward the center of the long side of the board. It would travel forward and turn right, back up and align itself against the wall. Then it would score the two little balls along the short side, moving back and forth at the goal to push the balls in. Occasionally, when our robot failed to score, it would scoop up the missed ball with it's claw on the next part. After attempting to score the two balls, we went along the long side of the board, trying to gather five more balls to score. It would return, realign itself, and attempt to score by moving back and forth. The first two ensure that if we are messed up in the middle of scoring the five balls, we will at least have those two. After scoring a maximum of 7 balls, it would align itself to run down the long side of the board, travel down, realign along the short side, and travel to the flagbox to raise the flag.

Files

umain.c -- has the setup and the call to the routines
defines.h -- defines for motors / sensors and constants
common.c -- common / misc functions
drive.c -- code for turning and basic driving
claw.c -- code for controlling the movement of the claw
pid.c -- PID code for driving straight
routines.c -- routines for the robot to run

umain.c

#include 
#include "defines.h"
#include 
#include "common.c"
#include "drive.c"
#include "claw.c"
#include "pid.c"
#include "routines.c"

uint8_t team_number[2] = {12,0};

// usersetup
int usetup (void) {
	printf_P (PSTR("\nPlace robot,    press go."));
	go_click ();
	printf_P (PSTR("\nStabilizing..."));
	pause (500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_MS_PER_DEG, 500L);
	
	while (!stop_press())
	{
		printf("\nFL:%03d  FR:%03d  BL:%03d  BR:%03d", IR_FL, IR_FR, IR_BL, IR_BR);
		pause(50);
	}
	
	return 0;
}

/**
 * Straight line
 */
int umain (void) {

	uint32_t master_time = get_time();
	detect_orientation();
	
	score_two();
	score_west();
	printf("\ntime: %06d    ", get_time() - master_time);
	go_for_flag();
	
	return 0;
}

defines.h

#ifndef __INCLUDE_USER_DEFINES_H__
#define __INCLUDE_USER_DEFINES_H__

// motor port defines
#define MOTOR_LEFT			0
#define MOTOR_RIGHT			1
#define MOTOR_FLAG			2

// servo defines
#define SERVO_STEER			0
#define SERVO_RIGHT_CLAW	1
#define SERVO_LEFT_CLAW		2
#define SERVO_DUMP			3
#define SERVO_RC_OPEN_MAX	511
#define SERVO_LC_OPEN_MAX	0
#define SERVO_RC_OPEN		472
#define SERVO_LC_OPEN		57
#define SERVO_RC_SCORE		365
#define SERVO_LC_SCORE		210
#define SERVO_RC_CLOSE		264
#define SERVO_LC_CLOSE		249
#define SERVO_RC_CLOSE_MAX	158
#define SERVO_LC_CLOSE_MAX	361

// analog defines
// gyro business
#define GYRO_PORT 			11
#define LSB_MS_PER_DEG		1270.0 // gyro goes neg as turn cw
// IR business
#define IR_FR_PORT			12
#define IR_FL_PORT			14
#define IR_BR_PORT			16
#define IR_BL_PORT			18
#define IR_BLACK_THRESH		300
#define IR_FR_BLACK_THRESH	100
#define IR_FL_BLACK_THRESH	100
#define IR_BR_BLACK_THRESH	100
#define IR_BL_BLACK_THRESH	100
#define IR_IS_BLACK(P, T)	((P > T) ? 1 : 0)
#define IR_FR				analog_read(IR_FR_PORT)
#define IR_FL				analog_read(IR_FL_PORT)
#define IR_BR				analog_read(IR_BR_PORT)
#define IR_BL				analog_read(IR_BL_PORT)

// encoder defines
#define ENCODER_LEFT_PORT	24
#define ENCODER_RIGHT_PORT	25
#define ENCODER_LEFT		encoder_read(ENCODER_LEFT_PORT)
#define ENCODER_RIGHT		encoder_read(ENCODER_RIGHT_PORT)

#define ENC_90				16
#define ENC_60				10
#define ENC_45				8

// speed defines
#define SPEED_FAST			250
#define SPEED_SCORE			200
#define SPEED_NORMAL		100
#define SPEED_STRAIGHT		100
#define SPEED_TURN			100
#define SPEED_BACK			-50
#define SPEED_ACC_MAX		10

// time / length defines
#define TIME_NORMAL_100		30
#define TIME_FAST_100		18
#define TIME_TURN			20

// drive direction
#define LEFT				451
#define RIGHT				451
#define STRAIGHT			216

// drive adjusting defines
#define MOTOR_ADJUST		1
#define MAX_ENCODER_DIFF	5
#define MAX_MOTOR_DIFF		20
#define MAX_MOTOR			250

// encoder distance defines
#define ENCODER_TO_WHEEL_RATIO 5 //???
#define WHEEL_CIRCUMFERENCE 27.5
#define WHEEL_TRACK 20.5

// dump defines
#define DUMP_UP				0
#define DUMP_DOWN			255
#define DUMP_PAUSE			100

// PID controps
#define PID_DELAY			50
#define KP					30
#define KI					3
#define KD					1

// motor current defines, speeds based on speed define names
#define CURRENT_NORMAL_STALL     170
#define CURRENT_FAST_STALL         700

#endif

common.c

float bounded(float proposed, float min, float max) {
	if (proposed < min) {
		return min;
	} else if (proposed > max) {
		return max;
	} else {
		return proposed;
	}
}

void flag_spin()
{
	motor_set_vel(MOTOR_FLAG, 250);
}

drive.c

#include 
#include 
#include "defines.h"

int16_t target_vel = 0;
int16_t right_vel = 0;
int16_t left_vel = 0;
/**
  * PRIORITIES
  * !!!a = steering properly (adjusting drive speed)
  * !!!b = change from using time to distances
  */
  
// variables to determine direction of encoder

/************************* DRIVE *************************/

/**
 * Sets right motor to vel
 */
void drive_right_motor(int16_t vel) {
	motor_set_vel(MOTOR_RIGHT, vel);
	right_vel = vel;
}

/**
 * Sets left motor to vel
 */
void drive_left_motor(int16_t vel) {
	motor_set_vel(MOTOR_LEFT, vel);
	left_vel = vel;
}

/**
 * Sets both motors to vel
 */
void drive_motors(int16_t vel) {
	motor_set_vel(MOTOR_LEFT, vel);
	motor_set_vel(MOTOR_RIGHT, vel);
	right_vel = left_vel = vel;
}

void accel(int16_t vel, uint32_t time)
{
	for (uint8_t i = 0; i < 5; i++)
	{
		drive_motors(vel * i / 5);
		pause(time / 5);
	}
}

/**
 * stops
 */
void drive_brake() {
	motor_brake(MOTOR_LEFT);
	motor_brake(MOTOR_RIGHT);
	right_vel = 0;
	left_vel = 0;
}

void drive_right_brake() {
	motor_brake(MOTOR_RIGHT);
}

void drive_left_brake() {
	motor_brake(MOTOR_LEFT);
}

void drive_coast()
{
	motor_set_vel(MOTOR_RIGHT, 0);
	motor_set_vel(MOTOR_LEFT,  0);
}


/************************* STEER *************************/

/**
 * Sets steer direction
 * 0 is right; 511 is left
 */
void steer(uint16_t pos) {

	// validify pos
	if (pos > 511) {
		pos = 511;
	} else if (pos < 0) {
		pos = 0;
	}
	
	// set steer servo
	servo_set_pos(SERVO_STEER, 511 - pos);
	pause(500);
}


/************************* TURNING *************************/

/**
 * Turns drive
 * Has timeout
 * -deg turns right; +deg turns left
 *
void turn(int16_t deg) {

	// get gyro info
	float gyro_init = gyro_get_degrees();
	
	//!!!a figure out how long to have timeout limit based on deg
	uint16_t timeout = 1000;
	uint16_t time = get_time();
	
	// brake
	drive_brake();
	
	// turn right
	if (deg < 0) {
		
		timeout = -deg * TIME_TURN;
		
		// set steer
		steer(RIGHT);
		
		// set motor speed
		drive_right_motor(-SPEED_TURN);
		drive_left_motor(SPEED_TURN);
		
		// turn until there or timeout
		while (gyro_get_degrees() - gyro_init > deg && get_time() - time < timeout) {}
	}
	
	// turn left
	else if (deg > 0) {
		
		timeout = deg * TIME_TURN;
		
		// set steer
		steer(LEFT);
		
		// set motor speed
		drive_right_motor(SPEED_TURN);
		drive_left_motor(-SPEED_TURN);
		
		// turn until there or timeout
		while (gyro_get_degrees() - gyro_init < deg && get_time() - time < timeout) { }
	}
	
	// brake
	drive_brake();
}


/**
 * Turns left 90 degrees
 *
void turn_left()
{
	turn(90);
}


/**
 * Turns right 90 degrees
 *
void turn_right()
{
	turn(-90);
} */

void turn_left_enc(uint16_t deg)
{
	steer(LEFT);
	
	encoder_reset(ENCODER_RIGHT_PORT);
	encoder_reset(ENCODER_LEFT_PORT);
	
	drive_right_motor(SPEED_TURN);
	drive_left_motor(-SPEED_TURN);
	
	uint32_t timeout = get_time() + (deg * TIME_TURN);
	uint16_t enc_max = 0;
	
	if (deg < 60)
	{
		enc_max = ENC_45;
	}
	else if (deg < 90)
	{
		enc_max = ENC_60;
	}
	else
	{
		enc_max = ENC_90;
	}
	
	while (get_time() < timeout && (ENCODER_RIGHT <= enc_max || ENCODER_LEFT <= enc_max))
	{
		if (ENCODER_RIGHT > enc_max)
		{
			drive_right_brake();
		}
		if (ENCODER_LEFT > enc_max)
		{
			drive_left_brake();
		}
	}
	
	drive_brake();
}

void turn_180()
{
	steer(LEFT);
	
	encoder_reset(ENCODER_RIGHT_PORT);
	encoder_reset(ENCODER_LEFT_PORT);
	
	drive_right_motor(SPEED_TURN);
	drive_left_motor(-SPEED_TURN);
	
	uint32_t timeout = get_time() + (180 * TIME_TURN);
	uint16_t enc_max = ENC_90 * 2 + 4;
	
	
	while (get_time() < timeout && (ENCODER_RIGHT <= enc_max || ENCODER_LEFT <= enc_max))
	{
		if (ENCODER_RIGHT > enc_max)
		{
			drive_right_brake();
		}
		if (ENCODER_LEFT > enc_max)
		{
			drive_left_brake();
		}
	}
	
	drive_brake();
}


void turn_right_enc(uint16_t deg)
{
	steer(RIGHT);
	
	encoder_reset(ENCODER_RIGHT_PORT);
	encoder_reset(ENCODER_LEFT_PORT);
	
	drive_right_motor(-SPEED_TURN);
	drive_left_motor(SPEED_TURN);
	
	uint32_t timeout = get_time() + (deg * TIME_TURN);
	uint16_t enc_max = 0;
	
	if (deg < 60)
	{
		enc_max = ENC_45;
	}
	else if (deg < 90)
	{
		enc_max = ENC_60;
	}
	else
	{
		enc_max = ENC_90;
	}
	
	while (get_time() < timeout && (ENCODER_RIGHT <= enc_max || ENCODER_LEFT <= enc_max))
	{
		if (ENCODER_RIGHT > enc_max)
		{
			drive_right_brake();
		}
		if (ENCODER_LEFT > enc_max)
		{
			drive_left_brake();
		}
	}
	
	drive_brake();
}




/************************* DRIVE STRAIGHT WITH ENCODERS *************************/

/**
 * Drives straight for time_limit
 * currently takes in time -- eventually take in distance?
 * !!!b change to distance
 */
void drive_straight(int16_t vel, uint16_t time_limit) {
	
	uint16_t time = get_time();
	int16_t diff;
	int16_t speed_left = vel;
	int16_t speed_right = vel;
	
	printf("IN DRIVE_STRAIGHT");
	
	// brake, steer
	drive_brake();
	pause(50);
	steer(STRAIGHT);
	
	// reset encoders
	encoder_reset(ENCODER_LEFT_PORT);
	encoder_reset(ENCODER_RIGHT_PORT);
	
	// drive
	time = get_time();
	drive_motors(vel);
	
	// while not timeout
	while (get_time() - time < time_limit)
	{
		diff = ENCODER_LEFT - ENCODER_RIGHT;
		
		// if left is going faster than right
		if (diff > MAX_ENCODER_DIFF)
		{
			// adjust right up if it's less than vel + 20
			if (speed_right - vel < MAX_MOTOR_DIFF)
			{
				speed_right += MOTOR_ADJUST;
			}
			// adjust left down otherwise
			else
			{
				speed_left -= MOTOR_ADJUST;
			}
		}
		
		// if right is going faster than left
		if (-diff > MAX_ENCODER_DIFF)
		{
			// adjust left up if it's less than vel + 20
			if (speed_left - vel < MAX_MOTOR_DIFF)
			{
				speed_left += MOTOR_ADJUST;
			}
			// adjust right down otherwise
			else
			{
				speed_right -= MOTOR_ADJUST;
			}
		}
		
		speed_left = (int16_t) bounded(speed_left, 0, vel + MAX_MOTOR_DIFF);
		speed_right= (int16_t) bounded(speed_right, 0, vel + MAX_MOTOR_DIFF);
		
		drive_right_motor(speed_right);
		drive_left_motor(speed_left);
	}
	
	drive_brake();
	pause(50);
}

claw.c

#include 
#include "defines.h"

/**
 * controls left claw open
 * 0 = closed, 255 = open
 */
void left_claw(uint16_t deg) {
	
	// validify deg
	if (deg > 255) {
		deg = 255;
	} else if (deg < 0) {
		deg = 0;
	}
	
	// set left claw
	servo_set_pos(SERVO_LEFT_CLAW, 255 - deg);
}


/**
 * controls right claw open
 * 0 = closed, 255 = open
 */
void right_claw(uint16_t deg) {

	// validify deg
	if (deg > 255) {
		deg = 255;
	} else if (deg < 0) {
		deg = 0;
	}
	
	servo_set_pos(SERVO_RIGHT_CLAW, deg);
}

void open_claw() {
	servo_set_pos(SERVO_RIGHT_CLAW, SERVO_RC_OPEN);
	servo_set_pos(SERVO_LEFT_CLAW,  SERVO_LC_OPEN);
}

void close_claw() {
	servo_set_pos(SERVO_RIGHT_CLAW, SERVO_RC_CLOSE);
	servo_set_pos(SERVO_LEFT_CLAW,  SERVO_LC_CLOSE);
}

void score_claw() {
	servo_set_pos(SERVO_RIGHT_CLAW, SERVO_RC_SCORE);
	servo_set_pos(SERVO_LEFT_CLAW,  SERVO_LC_OPEN); // LC_OPEN
}

void open_claw_max() {
	servo_set_pos(SERVO_RIGHT_CLAW, SERVO_RC_OPEN_MAX);
	servo_set_pos(SERVO_LEFT_CLAW, SERVO_LC_OPEN_MAX);
}

void close_claw_max() {
	servo_set_pos(SERVO_RIGHT_CLAW, SERVO_RC_CLOSE_MAX);
	pause(25);
	servo_set_pos(SERVO_LEFT_CLAW, SERVO_LC_CLOSE_MAX);
}

pid.c

float encoder_diff(void);

void ForwardApplyMV(float);
void BackwardApplyMV(float);
void LeftApplyMV(float);
void RightApplyMV(float);

float BoundedForwardVelocity(float);
float BoundedBackwardVelocity(float);
void SetRightVelocity(float);
void SetLeftVelocity(float);
void drive_pid(int16_t, uint32_t);

float encoder_diff()
{
	return (float) ENCODER_RIGHT - (float) ENCODER_LEFT;
}

float BoundedForwardVelocity(float ProposedVelocity)
{
	if (ProposedVelocity < 0)
	{
		return 0;
	}
	else if (ProposedVelocity > MAX_MOTOR)
	{
		return MAX_MOTOR;
	}
	else
	{
		return ProposedVelocity;
	}
}

float BoundedBackwardVelocity(float ProposedVelocity)
{
	if (ProposedVelocity < -MAX_MOTOR)
	{
		return -MAX_MOTOR;
	}
	else if (ProposedVelocity > 0)
	{
		return 0;	
	}
	else
	{
		return ProposedVelocity;	
	}
}

void SetRightVelocity(float vel)
{
	motor_set_vel(MOTOR_RIGHT, (int16_t)vel);
}

void SetLeftVelocity(float vel)
{
	motor_set_vel(MOTOR_LEFT, (int16_t)vel);
}

void ForwardApplyMV(float MV)
{
	SetRightVelocity(BoundedForwardVelocity(target_vel+MV/2));
	SetLeftVelocity(BoundedForwardVelocity(target_vel-MV/2));	
}

void BackwardApplyMV(float MV)
{
	SetRightVelocity(BoundedBackwardVelocity(target_vel-MV/2));
	SetLeftVelocity(BoundedBackwardVelocity(target_vel+MV/2));
}

void drive_pid(int16_t vel, uint32_t time_limit)
{
	encoder_reset(ENCODER_LEFT_PORT);
	encoder_reset(ENCODER_RIGHT_PORT);
	
	target_vel = vel;

	steer(STRAIGHT);

	struct pid_controller pid;
	
	if (vel > 0)
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&ForwardApplyMV);
	}
	else
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&BackwardApplyMV);	
	}
	pid.goal = 0.0;
	pid.enabled = 1;

	if (vel > SPEED_STRAIGHT)
	{
		
	}
	time_limit = get_time() + time_limit;
	
	
	while (get_time() < time_limit)
	{
		update_pid(&pid);

		pause(PID_DELAY);
	}

	drive_coast();

}

float encoder_distance()
{
	/* take average of encoder readings, divide by ticks per centimeter */
	float encoder_ticks = ((encoder_read(ENCODER_LEFT_PORT)+encoder_read(ENCODER_RIGHT_PORT))/2);
	float distance = encoder_ticks/TICKS_PER_CM;
	return distance;
	
}

void drive_pid_distance(int16_t vel, uint32_t distance_limit)
{
	// reset encoders
	encoder_reset(ENCODER_LEFT_PORT);
	encoder_reset(ENCODER_RIGHT_PORT);
	
	// set the velocity
	target_vel = vel;

	// steer straight
	steer(STRAIGHT);

	// construct PID
	struct pid_controller pid;
	
	if (vel > 0)
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&ForwardApplyMV);
	}
	else
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&BackwardApplyMV);	
	}
	pid.goal = 0.0;
	pid.enabled = 1;
	
	// set timeout
	uint32_t time_limit = 0;
	if (vel >= SPEED_FAST || vel <= -SPEED_FAST) {
		time_limit = get_time() + (TIME_FAST_100 * distance_limit);
	} else {
		time_limit = get_time() + (TIME_NORMAL_100 * distance_limit);
	}
	
	// run
	while ((uint32_t) encoder_distance() < distance_limit && get_time() < time_limit)
	{
		update_pid(&pid);

		pause(PID_DELAY);
	}

	drive_coast();

}

void drive_pid_accel(int16_t vel, uint32_t distance_limit)
{
	// reset encoders
	encoder_reset(ENCODER_LEFT_PORT);
	encoder_reset(ENCODER_RIGHT_PORT);
	
	// set the velocity
	target_vel = vel;

	// steer straight
	steer(STRAIGHT);

	// construct PID
	struct pid_controller pid;
	
	if (vel > 0)
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&ForwardApplyMV);
	}
	else
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&BackwardApplyMV);	
	}
	pid.goal = 0.0;
	pid.enabled = 1;
	
	// set timeout
	uint32_t time_limit = 0;
	uint32_t time = get_time();
	if (vel >= SPEED_FAST || vel <= -SPEED_FAST) {
		time_limit = get_time() + (TIME_FAST_100 * distance_limit);
	} else {
		time_limit = get_time() + (TIME_NORMAL_100 * distance_limit);
	}
	
	// run
	while ((uint32_t) encoder_distance() < distance_limit && get_time() < time_limit)
	{
		if (vel > 0 && target_vel < vel)
		{
			target_vel = (uint16_t) ((get_time() - time) / 500 + 1) * 50;
			if (target_vel > vel)
			{
				target_vel = vel;
			}
		}
		else if (vel < 0 && target_vel > vel)
		{
			target_vel = (uint16_t) ((get_time() - time) / 100) * -50;
			if (target_vel < vel)
			{
				target_vel = vel;
			}
		}
		
		update_pid(&pid);

		pause(PID_DELAY);
	}

	drive_coast();

}


// FUNCTION STARTS HERE

//drive straight and stop if stalled
uint16_t drive_pid_stall(int16_t vel, uint32_t distance_limit)
{
    encoder_reset(ENCODER_LEFT_PORT);

    encoder_reset(ENCODER_RIGHT_PORT);
   
    uint8_t number_loops = 10;
    uint16_t stall_threshold;
    uint32_t initial_time = get_time();
    uint16_t current_reading[number_loops];
    uint16_t current_average = 0;
    uint8_t counter = 0;
   
    if (vel == SPEED_NORMAL || vel == -SPEED_NORMAL)
    {
        stall_threshold = CURRENT_NORMAL_STALL;
        printf("NORMAL_STALL");
    }
    else if (vel == SPEED_FAST || vel == -SPEED_FAST)
    {
        stall_threshold = CURRENT_FAST_STALL;
        printf("FAST_STALL");
    }
	else
	{
		stall_threshold = 0;
	}
   
    target_vel = vel;

    steer(STRAIGHT);

    struct pid_controller pid;
   
    if (vel > 0)
    {
        init_pid(    &pid,
                (float)KP,
                (float)KI,
                (float)KD,
                &encoder_diff,
                &ForwardApplyMV);
    }
    else
    {
        init_pid(    &pid,
                (float)KP,
                (float)KI,
                (float)KD,
                &encoder_diff,
                &BackwardApplyMV);   
    }
    pid.goal = 0.0;

    pid.enabled = 1;

    while ((uint32_t) encoder_distance() < distance_limit)
    {
        update_pid(&pid);
       
        if (counter < number_loops)         //averaging current readings over 5 loops
        {
            current_reading[counter] = ((motor_get_current_MA(MOTOR_LEFT)+motor_get_current_MA(MOTOR_RIGHT))/2);
            counter++;
        }
        else if (counter == number_loops)
        {
            for (uint8_t i=0; i stall_threshold) && ((get_time()-initial_time) > 500))
        {
            drive_coast();
            return 1;
        }

        pause(PID_DELAY);
        current_average = 0;
    }

    drive_coast();
    return 0;

}

void drive_pid_score(int16_t vel, uint32_t distance_limit)
{
	// reset encoders
	encoder_reset(ENCODER_LEFT_PORT);
	encoder_reset(ENCODER_RIGHT_PORT);
	
	// set the velocity
	target_vel = vel;

	// steer straight
	steer(STRAIGHT);

	// construct PID
	struct pid_controller pid;
	
	if (vel > 0)
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&ForwardApplyMV);
	}
	else
	{
		init_pid(	&pid,
				(float)KP,
				(float)KI,
				(float)KD,
				&encoder_diff,
				&BackwardApplyMV);	
	}
	pid.goal = 0.0;
	pid.enabled = 1;
	
	// set timeout
	uint32_t time_limit = 0;
	if (vel >= SPEED_FAST || vel <= -SPEED_FAST) {
		time_limit = get_time() + (TIME_FAST_100 * distance_limit);
	} else {
		time_limit = get_time() + (TIME_NORMAL_100 * distance_limit);
	}
	
	// run
	while ((uint32_t) encoder_distance() < distance_limit && get_time() < time_limit)
	{
		if ((uint32_t) encoder_distance() >= distance_limit / 2)
		{
			score_claw();
		}
		update_pid(&pid);

		pause(PID_DELAY);
	}

	drive_coast();

}

routines.c

#include 
#include "defines.h"
#include 

/**
 * Turn to face north
 */
void face_north(uint8_t color[], uint8_t is_odd) {
	if (color[0] == is_odd) {
		printf("FL IS ODD");
		turn_left_enc(90);
	} else if (color[1] == is_odd) {
		printf("FR IS ODD");
	} else if (color[2] == is_odd) {
		printf("BL IS ODD");
		turn_180();
	} else if (color[3] == is_odd) {
		printf("BR IS ODD");
		turn_right_enc(90);
	}
}

/**
 * Turn to fast east
 */
void face_east(uint8_t color[], uint8_t is_odd) {
	if (color[0] == is_odd) {
		
	} else if (color[1] == is_odd) {
		turn_right_enc(90);
	} else if (color[2] == is_odd) {
		turn_left_enc(90);
	} else if (color[3] == is_odd) {
		turn_180();
	} else {
		// !!!e BE CONFUSED AS HELL
	}
}

/**
 * DETCT ORIENTATION
 */
 
void detect_orientation()
{
	uint8_t color[4];
	uint8_t num_black = 0;
	uint8_t is_odd;
	
	color[0] = color[1] = color[2] = color[3] = 0;
	
	// if FL is black
	if (IR_IS_BLACK(IR_FL, IR_BLACK_THRESH) == 1)
	{
		num_black++;
		color[0] = 1;
	}
	
	if (IR_IS_BLACK(IR_FR, IR_BLACK_THRESH) == 1)
	{
		num_black++;
		color[1] = 1;
	}
	
	if (IR_IS_BLACK(IR_BL, IR_BLACK_THRESH) == 1)
	{
		num_black++;
		color[2] = 1;
	}
	
	if (IR_IS_BLACK(IR_BR, IR_BLACK_THRESH) == 1)
	{
		num_black++;
		color[3] = 1;
	}
	
	// figure out what color you're looking for
	is_odd = (num_black == 3) ? 0 : 1;
	
	// turn to face north
	face_north(color, is_odd);
}

int score_drive_thread()
{
	drive_pid_distance(SPEED_NORMAL, 40);
	return 0;
}

int score_claw_thread()
{
	score_claw();
	return 0;
}

void score_thread()
{
	create_thread(&score_drive_thread, STACK_DEFAULT, 0, "drive_thread");
	create_thread(&score_claw_thread,  STACK_DEFAULT, 0, "claw_thread");
}

void score_two()
{
	// drive forward
	drive_pid_distance(SPEED_NORMAL, 35);
	
	turn_right_enc(90);
	
	// back up into wall
	while (drive_pid_stall(-SPEED_NORMAL, 25) != 1);
	
	// collect balls
	open_claw();
	drive_pid_distance(SPEED_NORMAL, 145);
	close_claw_max();
	
	// score balls
	turn_right_enc(60);
	drive_pid_distance(-SPEED_NORMAL, 30);
	pause(300);
	drive_pid_score(SPEED_NORMAL, 43);
	pause(500);
	drive_pid_distance(-SPEED_NORMAL, 43);
	drive_pid_distance(SPEED_NORMAL, 43); // drive_pid_score(SPEED_NORMAL, 40);
}

void score_west()
{
	// get ready to go
	turn_left_enc(60);
	close_claw();
	turn_left_enc(90);
	
	// back up against wall
	while(drive_pid_stall(-SPEED_NORMAL, 35) != 1);
	open_claw();
	
	// collect west balls
	drive_pid_distance(SPEED_NORMAL, 165);
	close_claw(); //??? close claw max?
	
	// return 
	turn_180();
	drive_pid_distance(SPEED_NORMAL, 115);
	
	// line up straight
	turn_right_enc(90);
	while(drive_pid_stall(-SPEED_NORMAL, 50) != 1);
	
	// go back on track
	drive_pid_distance(SPEED_NORMAL, 45);
	turn_left_enc(90);
	
	// SCORE!
	turn_left_enc(45); //??? actual angle?
	close_claw_max();
	drive_pid_distance(-SPEED_NORMAL, 10);
	drive_pid_score(SPEED_NORMAL, 43);
	pause(700); // pause(500);
	drive_pid_distance(-SPEED_NORMAL, 43); // close_claw_max();
	close_claw_max();
	drive_pid_score(SPEED_NORMAL, 43); // drive_pid_distance(-SPEED_NORMAL, 40); 
	pause(500);
}

void wiggle()
{
	drive_right_motor(-50);
	pause(50);
	drive_coast();
	drive_left_motor(-50);
	pause(50);
	drive_coast();
	drive_right_motor(-50);
	pause(50);
	drive_coast();
	drive_left_motor(-50);
	pause(50);
	drive_coast();
}

void go_for_flag()
{
	// go for flagbox
	//straight self out
	close_claw_max();
	drive_pid_distance(-SPEED_NORMAL, 30);
	turn_left_enc(45);
	turn_left_enc(90);
	while(drive_pid_stall(-SPEED_NORMAL, 30) != 1);
	
	// go across board
	drive_pid_distance(SPEED_NORMAL, 170); // _NORMAL: 170; _fast 165
	turn_left_enc(90);
	
	// straighten out
	while(drive_pid_stall(-SPEED_NORMAL, 50) != 1);
	
	// go for flagbox
	drive_pid_distance(SPEED_NORMAL, 83);
	turn_right_enc(90);
	
	flag_spin();
	while(drive_pid_stall(-SPEED_NORMAL, 70) != 1);
	
	pause(1000);
	wiggle();
}

/**
 *Tests PID forward, backward and gyro turning
 */
void test_sequence()
{
	uint32_t time_start;
	
	time_start = get_time();
	drive_pid_accel(SPEED_FAST, 100);
	time_start = get_time() - time_start;
	
	printf("\n%05d %03d %03d   ",time_start, ENCODER_RIGHT, ENCODER_LEFT);
	
	time_start = get_time();
	drive_pid_accel(-SPEED_NORMAL, 100);
	time_start = get_time() - time_start;
	
	printf("%05d %03d %03d   ",time_start, ENCODER_RIGHT, ENCODER_LEFT);
	
	go_click();
	
	turn_right_enc(90);
	printf("\nR:%03d L:%03d   ", ENCODER_RIGHT, ENCODER_LEFT);
	pause(500);
	turn_left_enc(90);
	printf("R:%03d L:%03d   ", ENCODER_RIGHT, ENCODER_LEFT);
}

Home | Design | Code | Tips