RINGO STARR'S CODE

// Include headers from OS
#include
#include
#include
#include
//define sensors
#define GYRO_PORT 11
#define BUMP_WALL 1
#define FRONTRIGHT 15
#define BACKLEFT 14
#define BACKRIGHT 13
//define servo
#define servo 0
//define motors
#define MOTOR_LEFT 0
#define MOTOR_RIGHT 2
//define motor speeds
#define FAST 150
#define SLOW 0
#define BACK -100
#define TURN 100
int gyro_type = 300;
/* 150 for ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg = 2025.0;
/* nominal scale factor depending on gyro type */
float theta;
/* current angle, units explained in update_angle() */
float offset;
/* gyro offset */
float angle;
uint32_t time_ms;
/* system time the last gyro reading was taken */
volatile uint8_t bump;
/* Wall bump sensor*/
int FRlight;
/* light sensors */
int BLlight;
int BRlight;
int BL;
int BR;
int FR;
//define data
int White;
int Black;
int q;
/*differentiation between black and which*/
int i;
/*barrier between colors*/
int times_hitwall;
float a;
/*angle to 4 balls*/
void armup() {
servoSetPos(servo,206);
pause(20);
//printf("\narmup");
servoSetPos(servo,10);
void back_into_wall () {
motorSetVel(MOTOR_LEFT, -125);
motorSetVel(MOTOR_RIGHT, -125);
pause(1000);
motorSetVel(MOTOR_LEFT, -175);
//motorSetVel(MOTOR_RIGHT, 0);
pause(250);
//motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, -175);
pause(250);
motorSetVel(MOTOR_LEFT, -175);
//motorSetVel(MOTOR_RIGHT, 0);
pause(250);
//motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, -175);
pause(250);
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause(250);
}
void calibrate_gyro(void) {
/*
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;
uint32_t start_time_ms;
uint32_t int_time_ms = 5000L;
printf("\nPlace robot, pressgo.\n");
goClick();
printf("Stabilizing...\n");
pause(500); /* Wait for robot to stabilize mechanically */
printf("Calibrating offset...\n");
/* average some samples for offset */
sum = 0.0;
samples = 0;
start_time_ms = get_time();
while( ( get_time() - start_time_ms ) < int_time_ms ){
sum += (float)analogRead(GYRO_PORT);
samples++;
}
offset = sum / (float)samples;
theta = 0.0;
lcdClear();
//("Done calibration\n");
} /* calibrate_gyro() */
void gyro_cal(void) {
/*
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;
uint32_t start_time_ms;
uint32_t int_time_ms = 5000L;
//printf("Stabilizing...\n");
pause(500); /* Wait for robot to stabilize mechanically */
//printf("Calibrating offset...\n");
/* average some samples for offset */
sum = 0.0;
samples = 0;
start_time_ms = get_time();
while( ( get_time() - start_time_ms ) < int_time_ms ){
sum += (float)analogRead(GYRO_PORT);
samples++;
}
offset = sum / (float)samples;
theta = 0.0;
lcdClear();
//printf("Done calibration\n");
} /* calibrate_gyro() */
int update_angle(void) {
/* 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. */
uint32_t delta_t_ms, new_time_ms, analog_value;
time_ms = get_time();
for(;;){
analog_value = analogRead(GYRO_PORT);
new_time_ms = get_time();
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_value - offset) * (float)delta_t_ms;
time_ms = new_time_ms;
yield();
}
return 0;
} /* update_angle() */
void spin(int speed){
motorSetVel(MOTOR_LEFT, -speed);
motorSetVel(MOTOR_RIGHT, speed);
} /* spin() */
int sign(float a){
if ( a > 0.0 ) return 1;
else return -1;
} /* sign() */
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;
tolerance = 1.0 * lsb_ms_per_deg;
angle_target = angle * lsb_ms_per_deg;
error = theta - angle_target;
if ( fabs(error) > tolerance ) {
spin(180 * -sign(error));
while(fabs(error) > (80.0 * lsb_ms_per_deg)){
error = theta - angle_target;
yield();
}
spin(140 * -sign(error));
while(fabs(error) > (60.0 * lsb_ms_per_deg)){
error = theta - angle_target;
yield();
}
spin(120 * -sign(error));
while(fabs(error) > (10.0 * lsb_ms_per_deg)){
error = theta - angle_target;
yield();
}
/* Fine adjust either direction as necessary */
spin(100 * -sign(error));
while(fabs(error) > tolerance){
error = (theta - angle_target);
spin(100 * -sign(error));
}
spin(0);
} /* if ( fabs(error)... */
} /* go_to_angle() */
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 255 - tweak. */
uint32_t start_time;
uint8_t tweak = 30;
float angle_target;
float tolerance;
tolerance = lsb_ms_per_deg * 1.0; /* +/-0.5 degree accuracy */
/* first turn the robot */
go_to_angle(angle) ;
/* then go forward under gyro control for the specified time */
start_time = get_time();
angle_target = angle * lsb_ms_per_deg;
// while(bump == 0)
while ((get_time() - start_time) < duration_ms){
if (theta < (angle_target - tolerance)) {
/* Turn CCW a little */
motorSetVel(MOTOR_LEFT, speed - tweak);
motorSetVel(MOTOR_RIGHT, speed + tweak);
} else if (theta > (angle_target + tolerance)){
/* Turn CW a little */
motorSetVel(MOTOR_LEFT, speed + tweak);
motorSetVel(MOTOR_RIGHT, speed - tweak);
} else {
motorSetVel(MOTOR_LEFT, speed);
motorSetVel(MOTOR_RIGHT, speed);
}
/* You can check a sensor here and terminate the while loop */
} /* while */
/* Throw it in reverse briefly to stop quickly */
motorSetVel(MOTOR_LEFT, -255);
motorSetVel(MOTOR_RIGHT, -255);
pause(20L);
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
} /* void go_straight() */
void go_straight_until_bump(float angle,int speed){
/* The robot turns to the specified angle, then goes straight
until sensor. Note that speed must be greater than tweak,
and less than 255 - tweak. */
uint8_t tweak = 20;
float angle_target = theta;
float tolerance;
tolerance = lsb_ms_per_deg * 1.0; /* +/-0.5 degree accuracy */
/* first turn the robot */
go_to_angle(angle);
/* then go forward under gyro control for the specified time */
angle_target = angle * lsb_ms_per_deg;
//not sure if this should be bump==0 ?!?!?!?!?!?!??!?!?!?!?!?!?
while(bump == 0){
if (theta < (angle_target - tolerance)) {
/* Turn CCW a little */
//printf("turning ccw %f \n", theta);
motorSetVel(MOTOR_LEFT, speed - tweak);
motorSetVel(MOTOR_RIGHT, speed + tweak);
} else if (theta > (angle_target + tolerance)){
/* Turn CW a little */
motorSetVel(MOTOR_LEFT, speed + tweak);
motorSetVel(MOTOR_RIGHT, speed - tweak);
//printf("\nleft motor: %dmA - right motor: %dmA", motorGetCurrentMA(MOTOR_LEFT), );
//printf("turning cw %f \n", theta);
} else {
motorSetVel(MOTOR_LEFT, speed);
motorSetVel(MOTOR_RIGHT, speed);
//clear LCD
lcdClear();
//write LCD than write
//printf("Drive Straight %f", theta);
}
}
motorSetVel(MOTOR_LEFT, -255);
motorSetVel(MOTOR_RIGHT, -255);
pause(20L);
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
}
void go_open_loop( int speed, long duration_ms ){
/* Drive both motors at a specified speed and duration. */
uint32_t start_time;
start_time = get_time();
while ((get_time() - start_time) < duration_ms){
motorSetVel(MOTOR_LEFT, speed);
motorSetVel(MOTOR_RIGHT, speed);
/* You can check a sensor here and terminate the while loop if necessary */
} /* while */
/* Throw it in reverse briefly to stop quickly */
motorSetVel(MOTOR_LEFT, -100);
motorSetVel(MOTOR_RIGHT, -100);
pause(20L);
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
} /* void go_open_loop() */
int calibrateInfrared() {
int FRlightb;
int BLlightb;
int BRlightb;
int FRlightw;
int BLlightw;
int BRlightw;
//defining black (0)
printf("\nPlace on Black and click Go");
goClick();
BLlightb = analogRead(BACKLEFT);
FRlightb = analogRead(FRONTRIGHT);
BRlightb = analogRead(BACKRIGHT);
printf("\nBLACK: bl=%d. fr=%d. br=%d.", BLlightb, FRlightb, BRlightb);
Black=(BLlightb+FRlightb+BRlightb)/3;
pause(1000);
//defining white
printf("\nPlace on White and click Go");
goClick();
BLlightw = analogRead(BACKLEFT);
FRlightw = analogRead(FRONTRIGHT);
BRlightw = analogRead(BACKRIGHT);
printf("\nWHITE: bl=%d. fr=%d. br=%d.", BLlightw, FRlightw, BRlightw);
White=(BLlightw+FRlightw+BRlightw)/3;
pause (1000);
//getting the barrier number
q=(Black-White)/2;
i=White+q;
printf("\nbarrier=%d.",i);
pause (1000);
return 0;
}
int pick_bw (void){
//check infrared sensors
// 2 is white
//3 is black
FRlight = analogRead(FRONTRIGHT);
if (FRlight < i){
FR=2;
}
else{
FR=3;
}
BLlight = analogRead(BACKLEFT);
if (BLlight < i){
BL=2;
}
else{
BL=3;
}
BRlight = analogRead(BACKRIGHT);
if (BRlight < i){
BR=2;
}
else{
BR=3;
}
return 0;
}
int orient(void){
if (BR==BL && BL==FR) {
printf("\nGood to go!");
pause (50);
a=0.0;
} else if(BR==BL && FR!=BR){
printf("\nTurn 90 degrees CW.");
pause (50);
go_to_angle(-89.0);
a=-89.0;
}
else if(FR==BL && BR!=FR){
printf("\nTurn 180 degrees.");
pause (50);
go_to_angle(-178.0);
a=-178.0;
}
else {
printf("\nTurn 90 degrees CCW.");
pause (3000);
go_to_angle(86.0);
a=86.0;
}
return 0;
}
int bump_update(void) {
//Infinite loop that checks the wall sensor
while (1) {
bump = digitalGet(BUMP_WALL);
//printf("\nbumps=%d",bump);
}
return 0;
}
int time_hit_wall() {
//infinite loop that counts number of time the robot hit something
times_hitwall = 0;
while (1){
//bump = digitalGet(BUMP_WALL);
if (bump==1) {
times_hitwall = times_hitwall+1;
//printf ("\ntimes hit = %d.",times_hitwall);
pause(1000);
}
}
return 0;
}
int plan (void) {
//The balls go in and compress the switch and then the arm comes down
while(1){
if (times_hitwall == 0){
//go for the 4 balls
go_straight_until_bump(a-4.0, 230);
//printf("\nGoing Straight");
} else if(times_hitwall == 1) {
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause (50);
//back up
motorSetVel(MOTOR_LEFT, -175);
motorSetVel(MOTOR_RIGHT, -175);
pause (200);
//stop
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause(100);
//printf("\nFirst Hit. Just pushed 4 balls");
//turn 180
go_straight_until_bump(a+180.0, 230);
}
else if (times_hitwall == 2) {
//printf("\nNow going for the 2 other balls");
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause (50);
//back up
motorSetVel(MOTOR_LEFT, -175);
motorSetVel(MOTOR_RIGHT, -175);
pause (280);
//stop
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause(100);
go_straight_until_bump(a + 90.0, 230);
}
else if (times_hitwall == 3) {
//printf("\nDropping 2 balls and wait");
//Stop
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause (50);
//back up
motorSetVel(MOTOR_LEFT, -175);
motorSetVel(MOTOR_RIGHT, -175);
pause (250);
//stop
motorSetVel(MOTOR_LEFT, 0);
motorSetVel(MOTOR_RIGHT, 0);
pause (100);
//turning
go_straight(a-87.0, 175, 1000L);
//back into wall
//back_into_wall ();
//gyro_cal();
//going to clear middle
//go_straight(0.0, 175, 1000L);
go_straight_until_bump(a, 230);
}
else if (times_hitwall == 4) {
motorBrake(MOTOR_LEFT);
motorBrake(MOTOR_RIGHT);
}
}
return 0;
}
int usetup (void) {
rf_set_team(26);
calibrateInfrared();
calibrate_gyro();
//printf("\nCLick GO when ready...");
return 0;
}
int umain(void){
pick_bw();
create_thread(&update_angle, 64, 0, "update angle");
orient();
armup();
pause(100);
create_thread(&bump_update, 64, 0, "bump");
create_thread(&time_hit_wall, 64, 0, "time hit wall");
plan();
return 0;
}