// Basic Left wall following code
void wallFollowingL()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 40){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(SPEED, -SPEED);
}
}
if (leftFrontIR > 30 )
{ set_motors(SPEED-10, SPEED+10); } //turn left
else if (leftFrontIR < 15 )
{ set_motors (SPEED+10, SPEED-10); } //turn right
else
{set_motors(SPEED, SPEED); } // Straight
}
}
// Basic Right wall following code
void wallFollowingR()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 45){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(20, -20);
}
}
else if (rightFrontIR > 30 && rightSideIR > 25)
{ set_motors(SPEED+30, SPEED-8); } //turn right
else if (rightFrontIR < 20 )
{ set_motors (SPEED-10, SPEED+25); } //turn left
else
{set_motors(SPEED+8, SPEED+8); } // Straight
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// go to stuff
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool goToAngle(int toGoAngle, float* locationData) {
//spin in place until facing angle. motors will turn at same speed, but at opposite directions
int motorDiffkp = 10;
int motorDiffMax = 200;
int maxAngle = 35;
//get psi so it is between 180 and -180, 0 being at one goal
int psi = (int)(locationData[0] * RAD_DEG_RATIO) - 90;
if (psi<-180) { psi +=360; }
if (psi>180) { psi -=360; }
//error in angle, factoring in rotational velocity should be taken care of by kalman filter
int theta_angle = (psi-toGoAngle);
int atAngleError = 10;
if (theta_angle > 180 ) { theta_angle -= 360; }
if (theta_angle < -180) { theta_angle += 360; }
//motorDiff is the difference in PWM between the two motors
int motorDiff = (int)((float)(motorDiffkp)*(float)(abs(theta_angle))/(float)maxAngle+minPWM-20);
if (abs(theta_angle) > maxAngle) { motorDiff = motorDiffMax; }
if (theta_angle >0) { set_motors( motorDiff, -motorDiff ); }
else { set_motors( -motorDiff, motorDiff ); }
if ( abs(theta_angle) < atAngleError) { atAngle = true; return true;}
else {atAngle = false; return false;}
}
static void motor_drive() {
if (g_motor_state.enabled) {
set_motors(0, g_motor_state.direction * g_motor_state.speed);
} else {
set_motors(0, 0);
}
}
//Calibrates the sensor
void calibrate(unsigned int *sensors, unsigned int *minv, unsigned int *maxv) {
//say something to the user
clear();
lcd_goto_xy(0, 0);
print(" Fluffy");
lcd_goto_xy(0, 1);
print("A=Go!");
//wait on the calibration button
wait_for_button_press(BUTTON_A);
//wait for the user to move his hand
delay_ms(500);
//activate the motors
set_motors(40, -40);
//take 165 readings from the sensors...why not?
int i;
for (i = 0; i < 165; i++) {
read_line_sensors(sensors, IR_EMITTERS_ON);
update_bounds(sensors, minv, maxv);
delay_ms(10);
}
//and turn the motors off, we're done
set_motors(0, 0);
delay_ms(750);
}
void turnToDirection(int turnDirection) {
adjustAngle();
int currentDir;
currentDir = currentDirection();
double target = (currentDir + turnDirection) * M_PI / 2;
while(fabs(currentHeading() - target) > turnThreshold) {
if(fabs(currentHeading() - target) < slowThreshold) {
if(currentHeading() > target) {
set_motors(-5,5);
}
if(currentHeading() < target) {
set_motors(5,-5);
}
} else {
if(currentHeading() > target) {
set_motors(-15,15);
}
if(currentHeading() < target) {
set_motors(15,-15);
}
}
calcPos();
}
set_motors(0,0);
adjustAngle();
usleep(10000);
}
int main(void) {
m_clockdivide(0);
setup_pins();
if ( RF_debug) {setupUSB();}
setup_timer_1();
setup_timer_3();
m_bus_init();
m_rf_open(chan,RX_add,p_length);
int timer_3_cnt = 0;
//sei();
set_motors(0,0);
while (1){
if (check(TIFR3,OCF3A)){
set(TIFR3, OCF3A);
timer_3_cnt++;
if ( timer_3_cnt % 10 ==0 ) {
timer_3_cnt=0;
m_green(TOGGLE);
}
if ( timer_3_cnt == 1000){ timer_3_cnt=0;}
}
if(new){
switch ( receive_buffer[11] ) {
case Single_Joy: single_joystick(); break;
case Double_Joy: double_joystick(); break;
case Tank_Mode: tank_driving(); break;
case Mario_Kart: Mario_Drive(); break;
default : set_motors(0,0); m_green(2);
}
if(RF_debug){ debug_rf(); }
}
}
}
// Turns according to the parameter dir, which should be 'L', 'R', 'S'
// (straight), or 'B' (back).
void turn(char dir, int angle)
{
int time = (long)((float)(angle*MSPER90/9.0));
switch(dir)
{
case 'L':
// Turn left.
set_motors(-40,40);
delay_ms(time);
break;
case 'R':
// Turn right.
set_motors(40,-40);
delay_ms(time);
break;
case 'B':
// Turn around.
set_motors(40,-40);
delay_ms(time*2);
break;
case 'S':
// Don't do anything!
break;
}
}
void turnLeft(int maxspd, int minspd, int degrees)
{
//causes robot to spin left to a certain angle.
int left1, left2, right1, right2, drotated = 0;
get_motor_encoders(&left1, &right1);
int turnLength = RIGHT_ANGLE_TURN_ENC * degrees / 90;
while(drotated < turnLength)
{
get_motor_encoders(&left2, &right2);
drotated = ((left1 - left2) + (right2 - right1)) / 2;
if(drotated < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(-(minspd + drotated / 2), minspd + drotated / 2);
}
else if(drotated < turnLength - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(-maxspd, maxspd);
}
else
{
//decelerate phase
set_motors(-(minspd + (turnLength - drotated) / 2), minspd + (turnLength - drotated) / 2);
}
positioncalc();
}
set_motors(0, 0);
}
// Basic Left wall following code
void wallFollowingL()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
//for(i = 0; i < 250; i++)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
printf("i = %i\n", i);
if ( rightFrontIR < 30 )
{ turnRight (90.0, 5);} // super turn right
else if (leftFrontIR > 30 && leftSideIR > 25)
{ set_motors(SPEED-15, SPEED+50);} // super turn left
else if (leftFrontIR > 38)
{ set_motors(SPEED, 30);} // too far, turn left
else if ( leftFrontIR < 18 )
{ set_motors(SPEED*1.5, SPEED);} // turn right
else
{set_motors(SPEED+5, SPEED+5); } // Straight
}
}
void update_circle_velocities(float error){
int pwr_change = format_velocity(fabs(error));
int dir = field_state.drive_direction;
if(field_state.substage == DRIVE_SUBSTAGE){
/*if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
if(current_vel == 0)
current_vel = target_vel;
}*/
if(error*dir > 0){
set_motors(dir*current_vel, dir*current_vel - dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
}
else{
set_motors(dir*current_vel - dir*pwr_change, dir*current_vel);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
}
}
else if(field_state.substage == TERRITORY_APPROACH_SUBSTAGE || field_state.substage == LEVER_APPROACH_SUBSTAGE || field_state.substage == DUMPING_SUBSTAGE || field_state.substage == PIVOT_SUBSTAGE || field_state.substage == LEVER_RETREAT_SUBSTAGE || field_state.substage == TERRITORY_RETREAT_SUBSTAGE){
if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
if(current_vel == 0){
//used to know when we started moving toward the lever/territory
field_state.stored_time = field_state.curr_time;
current_vel = target_vel;
}
}
if(error*dir > 0){
set_motors(dir*current_vel + dir*pwr_change, dir*current_vel - dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
}
else{
set_motors(dir*current_vel - dir*pwr_change, dir*current_vel + dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
}
}
}
// ### Experimental racing function
double race_to(double curr_coord[2], double x, double y){
double steering = 1.8; // Ratio between the speed of each wheels.
double error_margin_angle = 3; // Degrees
double speed = 70;
double dx = x - curr_coord[0];
double dy = y - curr_coord[1];
double targetA = atan2(dx, dy);
double dangle = targetA - face_angle;
dangle += (dangle > to_rad(180)) ? -to_rad(360) : (dangle < -to_rad(180)) ? to_rad(360) : 0;
double distance = fabs(sqrt(dx*dx + dy*dy));
// If we need to turn, perform steering maneuver
if(fabs(dangle) >= to_rad(error_margin_angle)){
if (to_degree(dangle) > 0){
set_motors( speed*steering, speed/steering);
}
else {
set_motors(speed/steering, speed*steering);
}
printf("Steering: %f \n", to_degree(dangle));
}
else { // Otherwise just go straight;
set_motors(speed, speed);
}
position_tracker(curr_coord);
return distance;
}
void turn90()
{
while( fabs(currentHeading() - (M_PI/2.0)) > turnThreshold)
{
if(fabs(currentHeading() - (M_PI/2.0)) < slowThreshold)
{
if(currentHeading() > (M_PI/2.0))
{
set_motors(-1,1);
}
if(currentHeading() < (M_PI/2.0))
{
set_motors(1,-1);
}
}
else
{
if(currentHeading() > (M_PI/2.0))
{
set_motors(-15,15);
}
if(currentHeading() < (M_PI/2.0))
{
set_motors(15,-15);
}
}
calcPos();
}
set_motors(0,0);
printf("Target : %f\n", M_PI/2.0);
}
void loop()
{
// Set LED green.
set_color(RGB(0, 1, 0));
// Spinup motors to overcome friction.
spinup_motors();
// Move straight for 2 seconds (2000 ms).
set_motors(kilo_straight_left, kilo_straight_right);
delay(2000);
// Set LED red.
set_color(RGB(1, 0, 0));
// Spinup motors to overcome friction.
spinup_motors();
// Turn left for 2 seconds (2000 ms).
set_motors(kilo_turn_left, 0);
delay(2000);
// Set LED blue.
set_color(RGB(0, 0, 1));
// Spinup motors to overcome friction.
spinup_motors();
// Turn right for 2 seconds (2000 ms).
set_motors(0, kilo_turn_right);
delay(2000);
// Set LED off.
set_color(RGB(0, 0, 0));
// Stop for half a second (500 ms).
set_motors(0, 0);
delay(500);
}
void moveBackwards(int maxspd, int minspd, int dist)
{
//speeds up, moves the robot backwards a certain distance, and slows down.
int left1, left2, right1, right2;
get_motor_encoders(&left1, &right1);
while(1)
{
get_motor_encoders(&left2, &right2);
int dtravelled = ((left1 - left2) + (right1 - right2)) / 2;
if(dtravelled < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(-(minspd + dtravelled / 2), -(minspd + dtravelled / 2));
}
else if(dtravelled < dist - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(-maxspd, -maxspd);
}
else if(dtravelled < dist)
{
//decelerate phase
set_motors(-(minspd + (dist - dtravelled) / 2), -(minspd + (dist - dtravelled) / 2));
}
else
{
break;
}
positioncalc();
}
set_motors(0, 0);
}
void Mario_Drive(){
int HLeft = ((*(int*)(&receive_buffer[6])) * 3 )/ 2;
if (receive_buffer[1]==1) // right
{ left = -MK_reverse ; right = -MK_reverse;}
else if (receive_buffer[0]==1 ) // left
{ left = MK_drive ; right = MK_drive;}
else {left = 0; right =0; set_motors(0,0); return;}
if ( left > -60 ){ // going forward
if (HLeft > 0){ // joystic is to the right
right -= HLeft;
}else { // joystic is to the left
left += HLeft;
}
}else { // going backword ( left < 0)
if (HLeft > 0){ // joystic is to the right
right += HLeft;
}else { // joystic is to the left
left -= HLeft;
}
}
set_motors(left,right);
}
/** driveAhead ************************************************************
* Drive Ahead to the specified distance given in inches
* @params distance -- the distance in inches
* ***********************************************************************/
void driveAhead(double distance) {
// motors forward for some time
set_motors(SPEED, SPEED);
delay_ms(WAIT_PER_INCH * distance);
set_motors(0,0);
// pause for a fraction of a second before continuing to smooth out the run
delay_ms(250);
}
void turn_right(int power, int angle)
{
int delay = (DELAY_PER_ANGLE_PER_POWER * angle)/power;
set_motors(power,-power);
delay_ms(delay);
set_motors(0,0);
}
void correctToStraight(int motorSpeed) {
if (get_side_ir_dist(LEFT) < 15) {
set_motors(motorSpeed + 3, motorSpeed);
}
if (get_side_ir_dist(RIGHT) < 15) {
set_motors(motorSpeed, motorSpeed + 3);
}
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
unsigned int sensors[5]; // an array to hold sensor values
// set up the 3pi
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
// Get the position of the line. Note that we *must* provide
// the "sensors" argument to read_line() here, even though we
// are not interested in the individual sensor readings.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);
if(position < 1000)
{
// We are far to the right of the line: turn left.
// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 255, so we are driving
// it at just about 40% of the max.
set_motors(0,100);
// Just for fun, indicate the direction we are turning on
// the LEDs.
left_led(1);
right_led(0);
}
else if(position < 3000)
{
// We are somewhat close to being centered on the line:
// drive straight.
set_motors(100,100);
left_led(1);
right_led(1);
}
else
{
// We are far to the left of the line: turn right.
set_motors(100,0);
left_led(0);
right_led(1);
}
}
// This part of the code is never reached. A robot should
// never reach the end of its program, or unpredictable behavior
// will result as random code starts getting executed. If you
// really want to stop all actions at some point, set your motors
// to 0,0 and run the following command to loop forever:
//
// while(1);
}
void reculer(int dist)
{
int i,n=50;
set_motors(0,0);
delay_ms(1000);
for(i=0;i<n;i++)
{
set_motors(-50,-50);
delay_ms(dist);
}
set_motors(0,0);
}
void avancer(char dist)
{
int i,n=50;
set_motors(0,0);
delay_ms(1000);
for(i=0;i<n;i++)
{
set_motors(50,50);
delay_ms(dist);
}
set_motors(0,0);
}
int parallel(double *curr_coord){
int i = 0;
double leftir;
double rightir;
int parallel = 0;
double error = 0.04 ;
double time = 30.0;
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
while (!parallel){
for ( i = 0; i < time; i++)
{
leftir += get_front_ir_dist(LEFT);
rightir+= get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
if(leftir/time > rightir/time + error){
set_motors(1, -1);
position_tracker(curr_coord);
for ( i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else if(rightir/time > leftir/time + error){
set_motors(-1, 1);
position_tracker(curr_coord);
for (i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else{
parallel = 1;
set_motors(0, 0);
}
printf(" ### Paralleling LEFTIR %f, RIGHTIR %f \n", leftir/time, rightir/time);
usleep(300000);
set_motors(0, 0);
position_tracker(curr_coord);
}
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
usleep(20000);
}
void initialize()
{
play_from_program_space(welcome);
#ifdef DEBUG
// start receiving data at 9600 baud.
serial_set_baud_rate(9600);
serial_receive_ring(buffer, 100);
#endif
// initialize your QTR sensors
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2};
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2, IO_C3, IO_C4, IO_C5, IO_D7, IO_D4};
unsigned char qtr_rc_pins[] = {IO_D4, IO_D7, IO_C5, IO_C4, IO_C3, IO_C2, IO_C1, IO_C0};
qtr_rc_init(qtr_rc_pins, 8, 2000, IO_D2); // 800 us timeout, emitter pin PD2
#ifdef DEBUG
serial_send_blocking("Press Button A to start calibrating...\n", 39);
#endif
wait_for_button_press(BUTTON_A);
// Always wait for the button to be released so that the robot doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_A);
delay_ms(800);
// then start calibration phase and move the sensors over both
// reflectance extremes they will encounter in your application:
// We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
unsigned int counter; // used as a simple timer
for(counter = 0; counter < 82; counter++)
{
if(counter < 20 || counter >= 60)
set_motors(60,-60);
else
set_motors(-60,60);
qtr_calibrate(QTR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);
#ifdef DEBUG
serial_send_blocking("Press Button A to start line following...\n", 42);
#endif
wait_for_button_press(BUTTON_A);
wait_for_button_release(BUTTON_A);
}
/* les fonctions pour diriger le robot */
void droit(int dist)
{
int i,n=7;
set_motors(0,0);
delay_ms(1000);
for(i=0;i<n;i++)
{
set_motors(30,-30);
delay_ms(dist);
}
set_motors(0,0);
}
void gauche(int dist)
{
int i,n=7;
set_motors(0,0);
delay_ms(1000);
for(i=0;i<n;i++)
{
set_motors(-30,30);
delay_ms(dist);
}
set_motors(0,0);
}
void get_command(){
comm_cmd = false;
state_req = rf_buffer[0];
m_red(OFF);
// int i;
//for ( i = 0 ; i < H_LEDS ; i++){
// m_port_clear(PORT_ADD,PORTH,i);
//}
if(state_req == commTest){ m_red(2); }
else if(state_req == play){ set_motors(800,800); new_play=true; }
else if(state_req == pause){ set_motors(0,0);}
}
void second_main_loop(int i)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
/** this method (implementation) tells the robot to move ahead
* with the given distance (as an argument).
*/
void driveAhead(double distance) {
// double feet = distance * 3.28084; // convert meters to feet
// double delay = feet / 0.625;
set_motors(45,45);
delay_ms(3200); // replace 3200 with delay * 1000 to get ms
set_motors(0,0);
/** it went 2 ft. at 45/255 speed in 3200ms.
* so, v = 2 ft. / 3.2 sec. = 0.625 ft/s. therefore for the
* future, we can use this value to calculate time.
*
* t = distance / 0.625
*/
}
uint8_t DeviceCommands::next_command(void)
{
static char c;
c = getchr();
if (c>0)
put(c);
if (parse()) {
if (strcmp(command.name, "echo") == 0)
echo(command.parameters[0]);
else if (strcmp(command.name, "set_pwm") == 0)
set_pwm((uint8_t)command.parameters[0], command.parameters[1]);
else if (strcmp(command.name, "set_motors") == 0)
set_motors(command.parameters[0], command.parameters[1], command.parameters[2], command.parameters[3]);
else if (strcmp(command.name, "set_led") == 0)
set_led((uint8_t)command.parameters[0], (uint8_t)command.parameters[1]);
else if (strcmp(command.name, "read_adc") == 0)
read_adc();
else if (strcmp(command.name, "stop") == 0)
stop();
// comm << cp.command.name<<"\t"<<cp.command.nparameters<<"\n";
}
return 0;
}
