| TEAM 3 |
6.270 - Autonomous Robot Design Competition - IAP 2005 |
Home | Strategy | Design | Code | Results | 6.270
The Code
/* Sensor Ports*/
int frontLeft=27, frontRight=29, backRight=31;
/* Servo Ports */
int frontServo=5, backServo=0;
/* Motor Ports */
int leftMotor = 1, rightMotor = 3;
/* Gyro Port */
int gyro_port = 19;
/* Position of my and opponent coordinates */
int myX, myY, oppoX, oppoY; /*e=distance tolerance*/
/* Speed of forward progress */
int speed=100;
/* Threshold between black and white */
int flThreshold=80, frThreshold=80, brThreshold=20;
int flColor, frColor, brColor;
/* color of our bot 0=white, 1=black, and the rfID of our bot */
int selfColor=0, rfID=0;
int process_num, numTests=5;
/* Gyro variables */
int gyro_type = 300; /* 150 for ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg=256.0; /* nominal scale factor depending on gyro type */
float scale_factor = 1.; /* adjust this for your particular gyro! */
float theta; /* current angle, units explained in update_angle() */
float offset; /* gyro offset */
long time_ms;
void main() {
rf_team=3;
rf_enable();
calibrateInfrared();
start_machine();
/* printf("%d %d %d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight));
start_press();
/*
/* start_press();*/
process_num = start_process(update_angle());
/*start_press();*/
orient();
enable_servos();
step1();
step2();
step3();
motorStop();
}
void orient() {
calibrate_gyro();
/*determines the angle(theta) and color of itself*/
if(bwColor(frontLeft))
{
if(bwColor(frontRight))
{
if(bwColor(backRight))
{
selfColor = 1;
}
else
{
selfColor = 1;
turn90ccw();
}
}
else
{
if(bwColor(backRight))
{
printf("here");
sleep(5.0);
turn180();
selfColor = 1;
}
else
{
turn180();
selfColor = 0;
}
}
}
else
{
if(bwColor(frontRight))
{
if(bwColor(backRight))
{
turn90cw();
selfColor=1;
}
else
{
turn90ccw();
selfColor=0;
}
}
else
{
if(bwColor(backRight))
{
selfColor=0;
}
else
{
turn90cw();
selfColor=0;
printf("%d", selfColor);
}
}
}
/*figures out who it is on the rf*/
msleep(600L);
if(selfColor==0)
{
if(rf_y0<0)
rfID=0;
else
rfID=1;
}
else
{
if(rf_y0>0)
rfID=0;
else
rfID=1;
}
}
/* Return 0 if white, 1 if black */
int bwColor(int port)
{
if(port==frontLeft)
{
if(analogExp(port)>flThreshold)
return 1;
else
return 0;
}
if(port==frontRight)
{
if(analogExp(port)>frThreshold)
return 1;
else
return 0;
}
if(port==backRight)
{
if(analogExp(port)>brThreshold)
return 1;
else
return 0;
}
}
void closeFrontServo() {
servo(frontServo, 3800);
}
void closeRearServo() {
servo(backServo, 3800);
}
void openFrontServo() {
servo(frontServo, 200);
}
void openRearServo() {
servo(backServo, 200);
}
void turn90cw() {
calibrate_gyro();
go_to_angle(-90.);
motorStop();
}
void turn90ccw() {
calibrate_gyro();
go_to_angle(90.);
motorStop();
}
void turn180() {
calibrate_gyro();
go_to_angle(180.);
motorStop();
}
void motorStop()
{
motor(leftMotor, 0);
motor(rightMotor, 0);
msleep(100L);
}
/* scraps one reading of the port */
int analogExp(int port)
{
analog(port);
return analog(port);
}
void calibrateInfrared()
{
int i, flTotal, frTotal, brTotal;
int flBlack, frBlack, brBlack, flWhite, frWhite, brWhite;
/*CALIBRATING FOR BLACK IN ALL THREE SENSORS*/
printf("BLACK: fl=%d. fr=%d. br=%d.\n", frontLeft, frontRight, backRight);
start_press();
flTotal=frTotal=brTotal=0;
for(i=0; i<numTests; i++)
{
printf ("Values %d.%d.%d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight));
flTotal+=analogExp(frontLeft);
frTotal+=analogExp(frontRight);
brTotal+=analogExp(backRight);
msleep(100L);
}
flBlack=flTotal/numTests;
frBlack=frTotal/numTests;
brBlack=brTotal/numTests;
printf("BLACK=%d.%d.%d.\n", flBlack, frBlack, brBlack);
sleep(1.);
/*CALIBRATING FOR WHITE IN ALL THREE SENSORS*/
printf("WHITE: fl=%d. fr=%d. br=%d.\n", frontLeft, frontRight, backRight);
start_press();
flTotal=frTotal=brTotal=0;
for(i=0; i<numTests; i++)
{
printf ("Values %d.%d.%d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight));
flTotal+=analogExp(frontLeft);
frTotal+=analogExp(frontRight);
brTotal+=analogExp(backRight);
msleep(100L);
}
flWhite=flTotal/numTests;
frWhite=frTotal/numTests;
brWhite=brTotal/numTests;
printf("WHITE=%d.%d.%d.\n", flWhite, frWhite, brWhite);
sleep(1.);
flThreshold=(flBlack+flWhite)/2;
frThreshold=(frBlack+frWhite)/2;
brThreshold=(brBlack+brWhite)/2;
}
/* At the end of which front faces green*/
void step1() {
closeFrontServo();
closeRearServo();
calibrate_gyro();
go_straight(0., speed, 1000L);
/*CHANGED SO THAT IT DOES NOT RECALIBRATE AFTER PUSHING THE 4 BALLS IN*/
motorStop();
msleep(500L);
/* calibrate_gyro();*/
if (selfColor==0) {
/* turn90cw();*/
go_to_angle(-90.);
motorStop();
}
else {
/*turn90ccw();*/
go_to_angle(90.);
motorStop();
}
openFrontServo();
openRearServo();
}
/* Ends with the bot facing vertical */
void step2() {
/* calibrate_gyro();*/
float a;
if(selfColor==0)
a=-90.;
if(selfColor==1)
a=90.;
if (rfID==0) {
if (rf_x1>=0) {
go_straight(a, -speed, 3000L);
calibrate_gyro();
go_straight(0., speed, 6000L);
motorStop();
go_straight(0., -speed, 250L);
if (rf_y0>0) {
turn90ccw();
go_straight(90., -speed, 2000L);
calibrate_gyro();
go_straight(0., speed, 8500L);
}
else {
turn90cw();
go_straight(-90., -speed, 2000L);
calibrate_gyro();
go_straight(0., speed, 8500L);
}
}
else {
go_straight(a, speed, 3000L);
calibrate_gyro();
go_straight(0., -speed, 6000L);
motorStop();
go_straight(0., speed, 250L);
if (rf_y0 >0) {
turn90cw();
go_straight(-90., speed, 2000L);
calibrate_gyro();
go_straight(0., -speed, 8500L);
}
else {
turn90ccw();
go_straight(90., speed, 2000L);
calibrate_gyro();
go_straight(0., -speed, 8500L);
}
}
}
else {
if (rf_x0>=0) {
go_straight(a, -speed, 3000L);
calibrate_gyro();
go_straight(0., speed, 6000L);
motorStop();
go_straight(0., -speed, 250L);
if (rf_y1 >0) {
turn90ccw();
go_straight(90., -speed, 2000L);
calibrate_gyro();
go_straight(0., speed, 8500L);
}
else {
turn90cw();
go_straight(-90., -speed, 2000L);
calibrate_gyro();
go_straight(0., speed, 8500L);
}
}
else {
go_straight(a, speed, 3000L);
calibrate_gyro();
go_straight(0., -speed, 6000L);
motorStop();
go_straight(0., speed, 250L);
if (rf_y1 >0) {
turn90cw();
go_straight(-90., speed, 2000L);
calibrate_gyro();
go_straight(0., -speed, 8500L);
}
else {
turn90ccw();
go_straight(90., speed, 2000L);
calibrate_gyro();
go_straight(0., -speed, 8500L);
}
}
}
motorStop();
}
/* Dumps the front compartment into our scoring area and vote with the four of the other color, */
void step3() {
calibrate_gyro();
if (rfID==0) {
if (rf_y0>=0 && rf_x0>=0) {
closeRearServo();
msleep(500L);
go_straight(0., speed, 2800L);
openRearServo();
turn90ccw();
calibrate_gyro();
closeFrontServo();
msleep(1000L);
go_straight(0., -speed, 1000L);
go_straight(0., speed, 4000L);
}
if (rf_y0<0 && rf_x0>=0) {
closeRearServo();
msleep(500L);
go_straight(0., speed, 2800L);
openRearServo();
turn90cw();
calibrate_gyro();
closeFrontServo();
msleep(1000L);
go_straight(0., -speed, 1000L);
go_straight(0., speed, 4000L);
}
if (rf_y0>=0 && rf_x0<0) {
closeFrontServo();
msleep(500L);
go_straight(0., -speed, 2800L);
openFrontServo();
turn90cw();
calibrate_gyro();
closeRearServo();
msleep(1000L);
go_straight(0., speed, 1000L);
go_straight(0., -speed, 4000L);
}
if (rf_y0<0 && rf_x0<0) {
closeFrontServo();
msleep(500L);
go_straight(0., -speed, 2800L);
openFrontServo();
turn90ccw();
calibrate_gyro();
closeRearServo();
msleep(1000L);
go_straight(0., speed, 1000L);
go_straight(0., -speed, 4000L);
}
}
if (rfID==1) {
if (rf_y1>=0 && rf_x1>=0) {
closeRearServo();
msleep(500L);
go_straight(0., speed, 2800L);
openRearServo();
turn90ccw();
calibrate_gyro();
closeFrontServo();
msleep(1000L);
go_straight(0., -speed, 1000L);
go_straight(0., speed, 4000L);
}
if (rf_y1<0 && rf_x1>=0) {
closeRearServo();
msleep(500L);
go_straight(0., speed, 2800L);
openRearServo();
turn90cw();
calibrate_gyro();
closeFrontServo();
msleep(1000L);
go_straight(0., -speed, 1000L);
go_straight(0., speed, 4000L);
}
if (rf_y1>=0 && rf_x1<0) {
closeFrontServo();
msleep(500L);
go_straight(0., -speed, 2800L);
openFrontServo();
turn90cw();
calibrate_gyro();
closeRearServo();
msleep(1000L);
go_straight(0., speed, 1000L);
go_straight(0., -speed, 4000L);
}
if (rf_y1<0 && rf_x1<0) {
closeFrontServo();
msleep(500L);
go_straight(0., -speed, 2800L);
openFrontServo();
turn90ccw();
calibrate_gyro();
closeRearServo();
msleep(1000L);
go_straight(0., speed, 1000L);
go_straight(0., -speed, 4000L);
}
}
}
/* Gyro support code */
void calibrate_gyro(){
/*
Averaging a number of readings only works when the gyro
noise is greater than the converter resolution. Since
the xrs300 noise is far lower than the 20mV resolution
of the 8-bit converter, a noise source has been added
to the gyro board. You should average for about 5
seconds to minimize the effects of the added noise.
*/
int samples;
float sum;
long start_time_ms;
long int_time_ms = 1000L;
kill_process(process_num);
printf("Stabilizing...\n");
msleep(200L); /* Wait for robot to stabilize mechanically */
printf("Calibrating offset...\n");
/* average some samples for offset */
sum = 0.0;
samples = 0;
start_time_ms = mseconds();
while( ( mseconds() - start_time_ms ) < int_time_ms ){
sum += (float)analog(gyro_port);
samples++;
}
offset = sum / (float)samples;
theta = 0.0;
process_num = start_process(update_angle());
} /* calibrate_gyro() */
void update_angle(){
/* The units of theta are converter LSBs * ms
1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for ADXRS150
1 deg = 0.256LSB * 1000ms = 256 (stored in lsb_ms_per_deg)
To convert theta to degrees, divide by lsb_ms_per_deg. */
long delta_t_ms, new_time_ms;
int i;
time_ms = mseconds();
for(;;){
new_time_ms = mseconds();
delta_t_ms = (new_time_ms - time_ms);
/* This does the Riemann sum; CCW gyro output polarity is negative
when the gyro is visible from the top of the robot. */
theta -= ((float)analog(gyro_port) - offset) * (float)delta_t_ms;
time_ms = new_time_ms;
defer();
}
} /* update_angle() */
void go_to_angle(float angle) {
/* To convert degrees to units of theta, multiply by lsb_ms_per_deg.
Demo robot turns at about 120 deg/s when speed = 100 */
float angle_target;
float error, tolerance;
int speed;
tolerance = 1.0 * lsb_ms_per_deg;
angle_target = angle * lsb_ms_per_deg;
error = theta - angle_target;
/*MAKES THE ROBOT TURN THE SHORTEST ARC*/
while(fabs(theta-angle_target-360.*lsb_ms_per_deg) < fabs(error))
{
angle_target+=360.*lsb_ms_per_deg;
error=theta-angle_target;
}
while(fabs(theta-angle_target+360.*lsb_ms_per_deg) < fabs(error))
{
angle_target-=360.*lsb_ms_per_deg;
error=theta-angle_target;
}
if ( fabs(error) > tolerance ) {
spin(90 * -sign(error));
while(fabs(error) > (80.0 * lsb_ms_per_deg)){
error = theta - angle_target;
}
spin(75 * -sign(error));
while(fabs(error) > (60.0 * lsb_ms_per_deg)){
error = theta - angle_target;
}
spin(70 * -sign(error));
while(fabs(error) > (10.0 * lsb_ms_per_deg)){
error = theta - angle_target;
}
/* Slam on the brakes */
spin(100 * sign(error));
msleep(10L);
/* Fine adjust either direction as necessary */
spin(50 * -sign(error));
while(fabs(error) > tolerance){
error = (theta - angle_target);
spin(50 * -sign(error));
}
} /* if ( fabs(error)... */
} /* go_to_angle() */
void spin(int speed){
motor(leftMotor, -speed);
motor(rightMotor, speed);
} /* spin() */
int sign(float a){
if ( a > 0.0 ) return 1;
else return -1;
} /* sign() */
float fabs(float a){
if ( a > 0.0 ) return a;
else return -a;
} /* fabs() */
void go_straight(float angle, int speed, long duration_ms){
/* The robot turns to the specified angle, then goes straight for
the specified duration. Note that speed must be greater than tweak,
and less than 100 - tweak. */
long start_time;
int tweak = 30;
float angle_target;
float tolerance;
tolerance = lsb_ms_per_deg * 1.; /* +/-0.5 degree accuracy ***************CHANGED */
/* first turn the robot */
go_to_angle(angle) ;
/* then go forward under gyro control for the specified time */
start_time = mseconds();
angle_target = angle * lsb_ms_per_deg;
while ((mseconds() - start_time) < duration_ms){
if (theta < (angle_target - tolerance)) {
/* Turn CCW a little */
motor(leftMotor, speed - tweak);
motor(rightMotor, speed + tweak);
} else if (theta > (angle_target + tolerance)){
/* Turn CW a little */
motor(leftMotor, speed + tweak);
motor(rightMotor, speed - tweak);
} else {
motor(leftMotor, speed);
motor(rightMotor, speed);
}
/* You can check a sensor here and terminate the while loop */
} /* while */
motorStop();
} /* void go_straight() */