/* Gyro demo program for gyro with noise injection. Howard Samuels, 11/16/2004 howard.samuels@analog.com Modified by Team 13. */ 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 = 10000L; printf("\nPlace robot, press start."); start_press(); printf("\nStabilizing..."); threadsafe_msleep(500L); /* Wait for robot to stabilize mechanically */ printf("\nCalibrating offset..."); /* average some samples for offset */ sum = 0.0; samples = 0; start_time_ms = mseconds(); while( ( mseconds() - start_time_ms ) < int_time_ms && samples < num_avgs){ sum += (float)analog(gyro_port); samples++; } /*printf("SUM: %f,Smpl:%d",sum,samples); msleep(4000L);*/ offset = sum / (float)samples; theta = 0.0; return; } /* 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; angle = theta / lsb_ms_per_deg; time_ms = new_time_ms; defer(); } return; } /* update_angle() */ /*void display_angle(){ for(;;){ printf("\n%d degrees",(int)(theta / lsb_ms_per_deg)); threadsafe_msleep(200L); defer(); } return; } /* display_angle() */ void gyro_go_to_angle(float angletarget, float timeout){ float TO_end_time = seconds() + timeout; while(fabs(angle - angletarget) > 1.5 && seconds() < TO_end_time){ if(fabs(angletarget- angle) > 30.0){ gyro_go_to_angle_large(angletarget, TO_end_time - seconds()); } else{ if(fabs(angletarget- angle) > 10.0){ gyro_go_to_angle_mid(angletarget, TO_end_time - seconds()); } else{ gyro_go_to_angle_small(angletarget, TO_end_time - seconds()); } } threadsafe_sleep(0.02); /* keep an eye out for overshoot */ } /* threadsafe_sleep(0.1); gyro_go_to_angle_small(angletarget, 1.5); */ last_was_straight = false; if(seconds() >= TO_end_time){ printf("\nTurn TO: time:%f",timeout); } return; } void gyro_go_to_angle_large(float angletarget, float timeout) { /* 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 TO_end_time = seconds() + timeout; float theta_target; float error, tolerance; int speed; if(angletarget<=angle) { /*turning to the right*/ servo(servoport,servoTURN+30); } if(angletarget>angle) { /*turning left*/ servo(servoport,servoTURN-100); } if(last_was_straight && seconds() < TO_end_time){ threadsafe_sleep(0.4); } last_was_straight = false; tolerance = 1.0 * lsb_ms_per_deg; theta_target = angletarget * lsb_ms_per_deg; error = theta - theta_target; if ( fabs(error) >= 30.0 && seconds() < TO_end_time) { spin(100 * -sign(error)); while(fabs(error) > (80.0 * lsb_ms_per_deg) && seconds() < TO_end_time){ error = theta - theta_target; /* printf("\nerr/lsb1: %f", error/lsb_ms_per_deg);*/ } spin(100 * -sign(error)); while(fabs(error) > (60.0 * lsb_ms_per_deg) && seconds() < TO_end_time){ error = theta - theta_target; /*printf("\nerr/lsb2: %f", error/lsb_ms_per_deg);*/ } spin(100 * -sign(error)); while(fabs(error) > (30.0 * lsb_ms_per_deg) && seconds() < TO_end_time){ error = theta - theta_target; /* printf("\nerr/lsb3: %f", error/lsb_ms_per_deg);*/ } /* SERVO BRAKE! - not the best of ideas, but works pretty well*/ /*servo(servoport,servoFWD); */ /* Slam on the brakes */ spin(50 * sign(error)); threadsafe_msleep(30L); error = theta - theta_target; spin(0); } return; } void gyro_go_to_angle_mid(float angletarget, float timeout) { /* To convert degrees to units of theta, multiply by lsb_ms_per_deg. Demo robot turns at about 12_0 deg/s when speed = 100 */ float TO_end_time = seconds() + timeout; float theta_target; float error, tolerance; int speed; if(angletarget<=angle) { /*turning to the right*/ servo(servoport,servoTURN+30); } if(angletarget>angle) { /*turning left*/ servo(servoport,servoTURN-100); } if(last_was_straight && seconds() < TO_end_time){ threadsafe_sleep(0.4); } last_was_straight = false; tolerance = 10.0 * lsb_ms_per_deg; theta_target = angletarget * lsb_ms_per_deg; error = theta - theta_target; if ( fabs(error) > tolerance && seconds() < TO_end_time) { while(fabs(error) > tolerance && seconds() < TO_end_time){ error = (theta - theta_target); spin(30 * -sign(error)); /* printf("\nerr/lsb5: %f", error/lsb_ms_per_deg);*/ } } spin(0); return; } void gyro_go_to_angle_small(float angletarget, float timeout) { /* To convert degrees to units of theta, multiply by lsb_ms_per_deg. Demo robot turns at about 12_0 deg/s when speed = 100 */ float TO_end_time = seconds() + timeout; float theta_target; float error, tolerance; int speed; if(angletarget<=angle) { /*turning to the right*/ servo(servoport,servoTURN+30); } if(angletarget>angle) { /*turning left*/ servo(servoport,servoTURN-100); } if(last_was_straight && seconds() < TO_end_time){ threadsafe_sleep(0.4); } last_was_straight = false; tolerance = 1.5 * lsb_ms_per_deg; theta_target = angletarget * lsb_ms_per_deg; error = theta - theta_target; if ( fabs(error) > tolerance && seconds() < TO_end_time) { while(fabs(error) > tolerance && seconds() < TO_end_time){ error = (theta - theta_target); bad_Lmotor_brake(10,-35*sign(error)); bad_Rmotor_brake(-10,35*sign(error)); threadsafe_sleep(0.001); /*spin(7 * -sign(error)); */ /* was power = 14 */ spin(0 * -sign(error)); threadsafe_sleep(0.001); /* printf("\nerr/lsb5: %f", error/lsb_ms_per_deg);*/ } } spin(0); return; } void spin(int speed){ Lmotor(-speed); Rmotor(speed); } /* spin() */