/********************************************************************** Run a balancing servo off a clock. Switch to SERVO_INTERVAL = 0.004 Filter the command, not the wheel velocity Add a dead zone on the command. Move to first order filtering on wheel angle to produce wheel velocity Add a launch behavior Commented out az, only getting ay now * Need to have "I2Cdev" folder in Arduino libraries * Need to have "MPU6050" folder in Arduino libraries /**********************************************************************/ #include "I2Cdev.h" #include "MPU6050.h" #include "Pins.h" #include "My_encoders.h" /**********************************************************************/ #define RUN_DURATION 30000 #define DO_SINUSOID 0 #define DO_MIN_JERK 0 #define MAX_ULONG 0xFFFFFFFF // I had to increase this to 4ms to accomodate more computation/tick // microseconds #define SERVO_INTERVAL 4000 // These are in seconds #define SERVO_INTERVAL_F (SERVO_INTERVAL*(1e-6)) #define ONE_OVER_SERVO_INTERVAL (1.0/SERVO_INTERVAL_F) // start data collection N milliseconds after starting servo #define START_COLLECT 5000 // start balancer N milliseconds after START_COLLECT #define START_BALANCE 100 // start safety checks N milliseconds after START_COLLECT #define START_SAFETY 1000 // start movements N milliseconds after START_BALANCE #define START_MOVEMENT 5000 // milliseconds, #define DEBUG_PRINT_INTERVAL 500 // The maximum command allowed #define MAX_COMMAND 255 // #define PI 3.141592653589793 #define ENCODER_TO_RADIANS 0.004027682889218 #define GSCALE_TO_ACC 0.001575 // convert Y accelerometer to radians #define ASCALE 5.575380291904716e-05 // convert gyro to radians/second #define GSCALE 1.352011295799561e-04 // this is 0.5*Ts*GSCALE // #define HALF_TS_GSCALE 2.0280e-07 // Ts = 0.003 #define HALF_TS_GSCALE 2.704022591599122e-07 // Ts = 0.004 // entries in check vector #define ENCODER_DIFFS_SUM2 0 #define ACCELEROMETER_SUM 1 #define GYRO_SUM 2 // Safety limit (radians) #define BODY_ANGLE_LIMIT 0.5 // Generic size of arrays #define NN 10 #define DO_NOT_COLLECT_DATA 0 #define PLEASE_COLLECT_DATA 1 #define MIN_JERK_MOVEMENT_SIZE (4.0*PI) /**********************************************************************/ // Enable for running a servo bool go = false; // Encoder readings: these can be positive or negative. long left_angle = 0; long right_angle = 0; // Wheel desired angle float drive_frequency = 0.0; float freq_2PI_over_1000 = 0; // time scaling factor float desired_angle_amplitude = 0; float desired_velocity_amplitude = 0; float wheel_desired_angle = 0; float wheel_desired_velocity = 0; // Wheel velocity variables float wheel_angle = 0; float last_wheel_angle = 0; // Command filter variables float past_raw_commands[NN]; float past_filtered_commands[NN]; MPU6050 accelgyro; // old approach to setting zeros int ax0 = 137; // 237; // 232; // 213; // 1253; // 231; // int ay0 = -590; // -975; // -661; // 22; // 260; // 589; // 219; // adjustment of az has to be done separately int gx0 = 345; // 346; // 354; // 351; // 333 // 329; // 331; int gy0 = 204; // 215; // 214; // 194; // 203; // 201; int gz0 = -211; // -208; // -216; // -190; // -195; // -192; // now hand adjusting ay0 int ay0 = -500; // body angle Kalman filter gain float Kf_body = 0.005; // 0.0001 and 0.001 too low, 0.1 too high // Used in body angle Kalman filter float last_body_angle = 0; long last_gxx = 0; // last zero adjusted X gyro reading // These two are used to handle the wierd gyro saturation behavior long last_gx = 0; // last raw X gyro reading long last_last_gx = 0; // previous to last raw X gyro reading // Keep track of late control cycles. unsigned long servo_late = 0; // How many times am I late? unsigned long max_servo_late = 0; // What was the worst one? // The servo gains /* // Original gains float wheel_angle_gain = 3.0697; // command/radian float wheel_angular_velocity_gain = 38.0003; // command/(radians/second) float body_angle_gain = 1055.6959; // command/radian float body_angular_velocity_gain = 113.1936; // command/(radians/second) // Q 1 100 1 1; R 0.1 - unstable? float wheel_angle_gain = 2.940576540698; // command/radian float wheel_angular_velocity_gain = 106.482698970935; // command/(radians/second) float body_angle_gain = 2538.632312883816; // command/radian float body_angular_velocity_gain = 299.026185181349; // command/(radians/second) // Q 1 100 1 1; R 1 float wheel_angle_gain = 0.962583708633; // command/radian float wheel_angular_velocity_gain = 51.701738210705; // command/(radians/second) float body_angle_gain = 1344.088947648885; // command/radian float body_angular_velocity_gain = 149.848009296931; // command/(radians/second) // Q 1 100 1 1; R 10 good, especially with -500 ay0 float wheel_angle_gain = 0.306904187685; // command/radian float wheel_angular_velocity_gain = 35.787021312886; // command/(radians/second) float body_angle_gain = 1010.702758072962; // command/radian float body_angular_velocity_gain = 108.024691090534; // command/(radians/second) // Q 1 100 1 1; R 1000000, collected data with these float wheel_angular_velocity_gain = 31.5997905221488; // command/(radians/second) float body_angle_gain = 926.2722415000157; // command/radian float body_angular_velocity_gain = 97.3938972633543; // command/(radians/second) // Q 100 1 1; R 1000000, new model float wheel_angular_velocity_gain = 23.6682434820394; // command/(radians/second) float body_angle_gain = 639.9856507938772; // command/radian float body_angular_velocity_gain = 94.5450730117037; // command/(radians/second) */ // Q 100 1 1; R 1000000, new new model float wheel_angular_velocity_gain = 22.82; // command/(radians/second) float body_angle_gain = 584.61; // command/radian float body_angular_velocity_gain = 92.69; // command/(radians/second float past_command_gain = 0.038; // command/command_units // override LQR designs above. float wheel_angle_gain = 10; // The wheel velocity low pass differentiating filter /* // From Matlab butter( 1, 0.001 ) float a_filter[2] = {1.000000000000000, -0.996863331833438}; float b_filter[2] = {0.001568334083281, 0.001568334083281}; // From Matlab butter( 1, 0.002 ) float a_filter[2] = {1.000000000000000, -0.993736471541615}; float b_filter[2] = {0.003131764229193, 0.003131764229193}; // From Matlab butter( 1, 0.005 ) float a_filter[2] = {1.000000000000000, -0.984414127416097}; float b_filter[2] = {0.007792936291952, 0.007792936291952}; // From Matlab butter( 1, 0.01 ) float a_filter[2] = {1.000000000000000, -0.969067417193793}; float b_filter[2] = {0.015466291403103, 0.015466291403103}; // From Matlab butter( 1, 0.02 ) float a_filter[2] = {1.000000000000000, -0.939062505817492}; float b_filter[2] = {0.030468747091254, 0.030468747091254}; // From Matlab butter( 1, 0.05 ) float a_filter[2] = {1.000000000000000, -0.854080685463467}; float b_filter[2] = {0.072959657268267, 0.072959657268267}; */ // From Matlab butter( 1, 0.1 ) float a_filter[2] = {1.000000000000000, -0.726542528005361}; float b_filter[2] = {0.136728735997319, 0.136728735997319}; /* // From Matlab butter( 1, 0.15 ) float a_filter[2] = {1.000000000000000, -0.612800788139932}; float b_filter[2] = {0.193599605930034, 0.193599605930034}; // From Matlab butter( 1, 0.2 ) float a_filter[2] = {1.000000000000000, -0.509525449494429}; float b_filter[2] = {0.245237275252786, 0.245237275252786}; // From Matlab butter( 1, 0.3 ) float a_filter[2] = {1.000000000000000, -0.324919696232906}; float b_filter[2] = {0.337540151883547, 0.337540151883547}; // From Matlab butter( 1, 0.4 ) float a_filter[2] = {1.000000000000000, -0.158384440324536}; float b_filter[2] = {0.420807779837732, 0.420807779837732}; // From Matlab butter( 1, 0.5 ) float a_filter[2] = {1.0, 0.0}; float b_filter[2] = {0.5, 0.5}; // From Matlab butter( 1, 0.7 ) float a_filter[2] = {1.000000000000000, 0.324919696232906}; float b_filter[2] = {0.662459848116453, 0.662459848116453}; // From Matlab butter( 1, 0.9 ) float a_filter[2] = {1.000000000000000, 0.726542528005361}; float b_filter[2] = {0.863271264002680, 0.863271264002680}; */ int launch_duration = 0; // Some checks to look at at the end of a run long checks[NN]; int check_counter = 0; int dead_zone = 0; // dead zone to deal with stiction float movement_duration = 1; float one_over_movement_duration = 1.0/movement_duration; float scaled_time = 0; int past_command = 0; /**********************************************************************/ /**********************************************************************/ // Controlling the motor void motor_init() { pinMode(AIN1, OUTPUT); pinMode(BIN1, OUTPUT); pinMode(PWMA_LEFT, OUTPUT); pinMode(PWMB_RIGHT, OUTPUT); digitalWrite(AIN1, HIGH); digitalWrite(BIN1, LOW); analogWrite(PWMA_LEFT, 0); analogWrite(PWMB_RIGHT, 0); pinMode(STBY_PIN, OUTPUT); digitalWrite(STBY_PIN, HIGH); } void motor_stop() { digitalWrite(AIN1, HIGH); digitalWrite(BIN1, LOW); analogWrite(PWMA_LEFT, 0); analogWrite(PWMB_RIGHT, 0); } void motor_left_command( int speed ) { if ( speed >= 0 ) { digitalWrite( AIN1, 1 ); analogWrite( PWMA_LEFT, speed ); } else { digitalWrite( AIN1, 0 ); analogWrite( PWMA_LEFT, -speed ); } } // reverses the sign of "speed" void motor_right_command( int speed ) { if ( speed >= 0 ) { digitalWrite( BIN1, 1 ); analogWrite( PWMB_RIGHT, speed ); } else { digitalWrite( BIN1, 0 ); analogWrite( PWMB_RIGHT, -speed ); } } /**********************************************************************/ // Initializations and instructions to user void setup() { Wire.begin(); // I2C (TWI) uses Wire library Serial.begin( 1000000 ); // run Serial fast to print out data Wire.setClock( 1000000UL ); // run I2C as fast as possible analogReference(INTERNAL1V1); // was INTERNAL on 328P, voltage while( !Serial ); // Wait for serial port to become ready Serial.println( "balance7 version 1" ); // print out what code is running delay(1000); // Delay to make sure above message prints out. motor_init(); // Initialize motors Serial.println( "motor_init done." ); delay(1000); // Delay to make sure above message prints out. Encoder_init( &left_angle, &right_angle ); // Initialize (zero) encoders Serial.println( "Initialized encoders" ); delay(1000); // Delay to make sure above message prints out. accelgyro.initialize(); // Initialize IMU Serial.println( "IMU initialized" ); delay(1000); // Delay to make sure above message prints out. Serial.println( "Robot should be standing up with training wheels." ); Serial.println( "Type g to run test, s to stop." ); Serial.println( "Typing window is at the top of the Arduino serial monitor window." ); Serial.println( "Type into the main window of a Putty serial monitor window." ); // code starts disabled, so every time Arduino powers up nothing happens go = false; } /******************************************************************/ // Get input from user void ProcessCommand() { if ( Serial.available() <= 0 ) return; int c = Serial.read(); switch (c) { case 'S': case 's': Serial.println( "Stop!" ); go = false; break; case 'G': case 'g': Serial.println( "Go!" ); go = true; break; default: break; } } /********************************************************************** Run a balancer with the given gains. run time starts at the beginning and is a millisecond clock. START_COLLECT starts data collection START_COLLECT milliseconds after "g" command. START_BALLANCE starts balancer (START_COLLECT+START_BALANCE) milliseconds after "g" command. DEBUG_PRINT_INTERVAL milliseconds between debug printouts when not doing data collection /**********************************************************************/ void run_balancer( int launch_duration, float wheel_angle_gain, float wheel_angular_velocity_gain, float body_angle_gain, float body_angular_velocity_gain, float past_command_gain, float some_kind_of_integral_gain, // placeholder unsigned long run_time_limit, int collect_data ) { // Clocks and time management // used to keep track of time intervals in servo unsigned long last_micros = micros(); unsigned long last_millis = millis(); int servo_time = 0; // Servo clock: needs to be signed, can be int // how long have we been running? Safety feature unsigned long run_time = 0; // milliseconds // Clocks for printing and other stuff unsigned long debug_print_time = 0; // milliseconds run_time_limit += START_COLLECT; // account for startup time // state int started = 0; // other initializations last_body_angle = 0; last_gx = 0; last_last_gx = 0; last_gxx = 0; int in_dead_zone = 0; last_wheel_angle = 0; for ( int i = 0; i < NN; i++ ) { past_raw_commands[i] = 0; past_filtered_commands[i] = 0; } // how many ticks has the servo executed (used to compute averages) check_counter = 0; float movement_start_time = START_COLLECT + START_BALANCE + START_MOVEMENT; wheel_desired_angle = 0; wheel_desired_velocity = 0; // keep track of voltage int voltage_raw = analogRead(VOL_MEASURE_PIN); // Read voltage value double voltage = (voltage_raw*1.1/1024)*((10+1.5)/1.5); if ( collect_data ) { Serial.print( "Current voltage " ); Serial.print( voltage ); Serial.print( " " ); Serial.println( voltage_raw ); } // print out the balancer gains if ( collect_data ) { Serial.print( "Gains " ); Serial.print( wheel_angle_gain, 8 ); Serial.print( " " ); Serial.print( wheel_angular_velocity_gain, 8 ); Serial.print( " " ); Serial.print( body_angle_gain, 8 ); Serial.print( " " ); Serial.print( body_angular_velocity_gain, 8 ); Serial.print( " " ); Serial.print( past_command_gain, 8 ); Serial.print( " " ); Serial.println( some_kind_of_integral_gain, 8 ); } // print out sinusoidal drive parameters if ( collect_data ) { Serial.print( "drive_frequency " ); Serial.println( drive_frequency ); Serial.print( "freq_2PI_over_1000 " ); Serial.println( freq_2PI_over_1000 ); Serial.print( "desired_angle_amplitude " ); Serial.println( desired_angle_amplitude ); Serial.print( "desired_velocity_amplitude " ); Serial.println( desired_velocity_amplitude ); } // print out other parameters if ( collect_data ) { Serial.print( "SERVO_INTERVAL " ); Serial.println( SERVO_INTERVAL ); Serial.print( "launch_duration " ); Serial.println( launch_duration ); Serial.print( "Kf_body " ); Serial.println( Kf_body, 8 ); Serial.print( "dead_zone " ); Serial.println( dead_zone ); Serial.println( "command filter_cutoff 0.1" ); } // print out IMU biases if ( collect_data ) { Serial.print( "IMU biases " ); Serial.print( ax0 ); Serial.print( " " ); Serial.print( ay0 ); Serial.print( " " ); Serial.print( gx0 ); Serial.print( " " ); Serial.print( gy0 ); Serial.print( " " ); Serial.println( gz0 ); } // servo loop for( ; ; ) { /***************************************** Timekeeping part */ unsigned long current_micros = micros(); if ( current_micros > last_micros ) servo_time += current_micros - last_micros; else // rollover servo_time += (MAX_ULONG - last_micros) + current_micros; last_micros = current_micros; // It isn't time yet ... if ( servo_time < SERVO_INTERVAL ) continue; // It is time! Reset the servo clock. servo_time -= SERVO_INTERVAL; // Keep track of maximum lateness if ( max_servo_late < servo_time ) max_servo_late = servo_time; // Let's have some slop in counting late cycles. if ( servo_time > 50 ) servo_late += 1; // handle the millisecond clocks unsigned long current_millis = millis(); int millis_increment; if ( current_millis > last_millis ) millis_increment = current_millis - last_millis; else // rollover millis_increment = (MAX_ULONG - last_millis) + current_millis; last_millis = current_millis; run_time += millis_increment; debug_print_time += millis_increment; /***************************************** Administrative part */ // Turn off after a while to keep from running forever if ( ( run_time > run_time_limit ) || ( !go ) ) { motor_stop(); Serial.println( "Motor stopped." ); Serial.println( "Time ran out." ); Serial.println( "Type g to run test, s to stop." ); return; } /***************************************** State estimation part */ // Read the sensors Read_encoders( &left_angle, &right_angle ); int enc_diff = left_angle - right_angle; checks[ENCODER_DIFFS_SUM2] += enc_diff*enc_diff; // convert to radians wheel_angle = -0.5*(ENCODER_TO_RADIANS)*((float) (left_angle + right_angle)); float wheel_velocity = ONE_OVER_SERVO_INTERVAL*(wheel_angle - last_wheel_angle); last_wheel_angle = wheel_angle; // Current accelerometer and gyro zero values. // int16_t ax, ay, az; // int16_t gx, gy, gz; // accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // accelgyro.getAcceleration( &ax, &ay, &az ); // This is type long so when add bias, don't flip sign long ay = (long) (accelgyro.getAccelerationY()); // This is type "long" to handle wierd negative saturation long gx = (long) (accelgyro.getRotationX()); // handle wierd negative saturation turns in to positive number on gyro if ( (last_gx <= last_last_gx) && ( last_gx < 0 ) && ( gx > 32000 ) ) gx = -32761; last_last_gx = last_gx; last_gx = gx; // subtract zeros: avoid sign flip by using longs // long axx = (long) ax - ax0; long ayy = (long) ay - ay0; // adjustment of az is not useful. // long azz = az; long gxx = (long) gx - gx0; // int gyy = gy - gy0; // int gzz = gz - gz0; checks[ ACCELEROMETER_SUM ] += ayy; checks[ GYRO_SUM ] += gxx; check_counter++; // prediction step float body_angle = last_body_angle + HALF_TS_GSCALE*((float) (gxx + last_gxx)); // measurement update body_angle -= Kf_body*(body_angle - (ASCALE)*((float) ayy)); last_body_angle = body_angle; last_gxx = gxx; // Safety code if ( run_time > START_COLLECT + START_BALANCE + START_SAFETY ) { if ( body_angle < -BODY_ANGLE_LIMIT ) { motor_stop(); Serial.println( "Motor stopped." ); Serial.print( "Robot fell backward:" ); Serial.print( " " ); Serial.println( body_angle ); Serial.println( "Type g to run test, s to stop." ); return; } if ( body_angle > BODY_ANGLE_LIMIT ) { motor_stop(); Serial.println( "Motor stopped." ); Serial.print( "Robot fell forward:" ); Serial.print( " " ); Serial.println( body_angle ); Serial.println( "Type g to run test, s to stop." ); return; } } float body_angular_velocity = (GSCALE)*gxx; /***************************************** Control part: */ float command_float = 0; int command = 0; if ( run_time < START_COLLECT + START_BALANCE ) { // Do nothing; command = 0; past_raw_commands[1] = command; past_raw_commands[0] = command; past_filtered_commands[0] = command; } else if ( run_time < START_COLLECT + START_BALANCE + launch_duration ) { command = -MAX_COMMAND; past_raw_commands[1] = command; past_raw_commands[0] = command; past_filtered_commands[0] = command; } else // Let's balance! { wheel_desired_angle = 0; wheel_desired_velocity = 0; float movement_time = (run_time - movement_start_time); // Drive the robot with sinusoids if ( movement_time > 0 && DO_SINUSOID ) { scaled_time = freq_2PI_over_1000*movement_time; // set up to go backwards first wheel_desired_angle = -desired_angle_amplitude*(1 - cos( scaled_time )); wheel_desired_velocity = -desired_velocity_amplitude*sin( scaled_time ); } // Drive the robot with minimum jerk movements static int min_jerk_direction = -1; static int min_jerk_old_integer_part = 0; if ( movement_time > 0 && DO_MIN_JERK ) { scaled_time = 0.001*movement_time*one_over_movement_duration; int integer_part = scaled_time; // integer conversion scaled_time -= integer_part; // get fractional part of float // now scaled time is between 0 and 1. // check if we need to flip direction if ( integer_part != min_jerk_old_integer_part ) { // we need to flip direction (start a new movement) min_jerk_direction = -min_jerk_direction; min_jerk_old_integer_part = integer_part; } float scaled_time2 = scaled_time*scaled_time; float scaled_time3 = scaled_time*scaled_time2; float scaled_time4 = scaled_time*scaled_time3; float scaled_time5 = scaled_time*scaled_time4; if ( min_jerk_direction > 0 ) { // positive direction sends us forward from -movement_size wheel_desired_angle = -MIN_JERK_MOVEMENT_SIZE + MIN_JERK_MOVEMENT_SIZE *(10.0*scaled_time3 - 15.0*scaled_time4 + 6.0*scaled_time5); wheel_desired_velocity = one_over_movement_duration*MIN_JERK_MOVEMENT_SIZE *(30.0*scaled_time2 - 60.0*scaled_time3 + 30.0*scaled_time4); } else { // negative direction sends us backward from 0 wheel_desired_angle = -MIN_JERK_MOVEMENT_SIZE *(10.0*scaled_time3 - 15.0*scaled_time4 + 6.0*scaled_time5); wheel_desired_velocity = -one_over_movement_duration*MIN_JERK_MOVEMENT_SIZE *(30.0*scaled_time2 - 60.0*scaled_time3 + 30.0*scaled_time4); } } // add integrator later command_float = - wheel_angle_gain * (wheel_angle - wheel_desired_angle) - wheel_angular_velocity_gain * (wheel_velocity - wheel_desired_velocity) - body_angle_gain*body_angle - body_angular_velocity_gain*body_angular_velocity - past_command_gain*past_command; // apply filter // a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) // - a(2)*y(n-1) - ... - a(na+1)*y(n-na) past_raw_commands[1] = past_raw_commands[0]; past_raw_commands[0] = command_float; float filtered_command = b_filter[0]*past_raw_commands[0] + b_filter[1]*past_raw_commands[1] - a_filter[1]*past_filtered_commands[0]; past_filtered_commands[0] = filtered_command; if ( filtered_command > MAX_COMMAND ) filtered_command = MAX_COMMAND; if ( filtered_command < -MAX_COMMAND ) filtered_command = -MAX_COMMAND; command = (int) filtered_command; // implement dead zone if ( dead_zone > 0 ) { if ( !in_dead_zone ) { // not already in dead zone, command positive if ( command > 0 ) { if ( command <= (dead_zone >> 1) ) { in_dead_zone = 1; command = 0; } } else // not already in dead zone, command negative { if ( command >= (-dead_zone >> 1) ) { in_dead_zone = 1; command = 0; } } } else { // already in dead zone if ( command > 0 ) { if ( command <= dead_zone ) command = 0; else in_dead_zone = 0; } else // in dead zone, command negative { if ( command >= -dead_zone ) command = 0; else in_dead_zone = 0; } } } } past_command = command; motor_left_command( command ); motor_right_command( command ); /***************************************** Data printing part */ // print out debugging info before data collection if ( ( debug_print_time > DEBUG_PRINT_INTERVAL ) && ( ( run_time < START_COLLECT ) || ( run_time > run_time_limit ) ) ) { Serial.print( servo_late ); Serial.print( " " ); Serial.print( max_servo_late ); Serial.print( " " ); Serial.print( ayy ); Serial.print( " " ); Serial.println( body_angle, 4 ); /* Serial.print( " " ); Serial.print( left_error ); Serial.print( " " ); Serial.print( right_error ); Serial.print( " " ); Serial.println( ramp ); */ ProcessCommand(); debug_print_time -= DEBUG_PRINT_INTERVAL; } // print out data if ( ( run_time >= START_COLLECT ) && ( run_time <= run_time_limit ) && collect_data ) { if ( !started ) { servo_late = 0; max_servo_late = 0; started = 1; Serial.println( "Data" ); } Serial.print( current_micros ); Serial.print( " " ); Serial.print( left_angle ); Serial.print( " " ); Serial.print( right_angle ); Serial.print( " " ); Serial.print( command ); Serial.print( " " ); Serial.print( command ); Serial.print( " " ); // Serial.print( axx ); // Serial.print( " " ); Serial.print( ayy ); // positive is tipping forward Serial.print( " " ); // Serial.print( azz ); // positive is down // Serial.print( " " ); Serial.print( gxx ); // positive is body rolling forward Serial.print( " " ); float v = 1000*body_angle; Serial.print( v ); Serial.print( " " ); Serial.println( wheel_velocity ); // Serial.print( scaled_time ); // Serial.print( " " ); // Serial.print( wheel_desired_angle ); // Serial.print( " " ); // Serial.println( wheel_desired_velocity ); // Serial.print( " " ); // Serial.println( gy ); // Serial.print( " " ); // Serial.println( gz ); // Serial.print( " " ); // Serial.println( left_velocity); // Serial.print( " " ); // Serial.println( right_velocity); // Serial.print( " " ); // Serial.print( left_angle_radians); // Serial.print( " " ); // Serial.println( left_error ); } } } /**********************************************************************/ void loop() { if ( !go ) { motor_stop(); delay(500); ProcessCommand(); return; } for ( int i = 0; i < NN; i++ ) checks[i] = 0; // keep track of voltage int voltage_raw = analogRead(VOL_MEASURE_PIN); // Read voltage value double voltage = (voltage_raw*1.1/1024)*((10+1.5)/1.5); Serial.print( "Volts: " ); Serial.println( voltage ); /* Serial.print( "Dead_zone (" ); Serial.print( dead_zone ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds int dead_zone_typed = Serial.parseInt(); if ( dead_zone_typed == 0 ) dead_zone_typed = dead_zone; Serial.print( "You typed: " ); Serial.println( dead_zone_typed ); if ( dead_zone_typed < 0 || dead_zone_typed > 300 ) { Serial.println( "Bad value: Start over" ); return; } dead_zone = dead_zone_typed; Serial.print( "Launch_duration (" ); Serial.print( launch_duration ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds int launch_duration_typed = Serial.parseInt(); if ( launch_duration_typed == 0 ) launch_duration_typed = launch_duration; Serial.print( "You typed: " ); Serial.println( launch_duration_typed ); if ( launch_duration_typed < 0 || launch_duration_typed > 300 ) { Serial.println( "Bad value: Start over" ); return; } launch_duration = launch_duration_typed; Serial.print( "Ay0 (" ); Serial.print( ay0 ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds long ay0_typed = Serial.parseInt(); if ( ay0_typed == 0 ) ay0_typed = ay0; Serial.print( "You typed: " ); Serial.println( ay0_typed ); if ( ay0_typed < -1000 || ay0_typed > 1000 ) { Serial.println( "Bad value: Start over" ); return; } ay0 = ay0_typed; Serial.print( "Wheel_angle_gain (" ); Serial.print( wheel_angle_gain ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float wheel_angle_gain_typed = Serial.parseFloat(); if ( wheel_angle_gain_typed == 0 ) wheel_angle_gain_typed = wheel_angle_gain; Serial.print( "You typed: " ); Serial.println( wheel_angle_gain_typed ); if ( wheel_angle_gain_typed < -100 || wheel_angle_gain_typed > 100 ) { Serial.println( "Bad value: Start over" ); return; } wheel_angle_gain = wheel_angle_gain_typed; Serial.print( "wheel_angular_velocity_gain (" ); Serial.print( wheel_angular_velocity_gain ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float wheel_angular_velocity_gain_typed = Serial.parseFloat(); if ( wheel_angular_velocity_gain_typed == 0 ) wheel_angular_velocity_gain_typed = wheel_angular_velocity_gain; Serial.print( "You typed: " ); Serial.println( wheel_angular_velocity_gain_typed ); if ( wheel_angular_velocity_gain_typed < -1000 || wheel_angular_velocity_gain_typed > 1000 ) { Serial.println( "Bad value: Start over" ); return; } wheel_angular_velocity_gain = wheel_angular_velocity_gain_typed; Serial.print( "Body_angle_gain (" ); Serial.print( body_angle_gain ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float body_angle_gain_typed = Serial.parseFloat(); if ( body_angle_gain_typed == 0 ) body_angle_gain_typed = body_angle_gain; Serial.print( "You typed: " ); Serial.println( body_angle_gain_typed ); if ( body_angle_gain_typed < -10000 || body_angle_gain_typed > 10000 ) { Serial.println( "Bad value: Start over" ); return; } body_angle_gain = body_angle_gain_typed; Serial.print( "body_angular_velocity_gain (" ); Serial.print( body_angular_velocity_gain ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float body_angular_velocity_gain_typed = Serial.parseFloat(); if ( body_angular_velocity_gain_typed == 0 ) body_angular_velocity_gain_typed = body_angular_velocity_gain; Serial.print( "You typed: " ); Serial.println( body_angular_velocity_gain_typed ); if ( body_angular_velocity_gain_typed < -1000 || body_angular_velocity_gain_typed > 1000 ) { Serial.println( "Bad value: Start over" ); return; } body_angular_velocity_gain = body_angular_velocity_gain_typed; Serial.print( "past_command_gain (" ); Serial.print( past_command_gain ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float past_command_gain_typed = Serial.parseFloat(); if ( past_command_gain_typed == 0 ) past_command_gain_typed = past_command_gain; Serial.print( "You typed: " ); Serial.println( past_command_gain_typed ); if ( past_command_gain_typed < -100 || past_command_gain_typed > 100 ) { Serial.println( "Bad value: Start over" ); return; } past_command_gain = past_command_gain_typed; */ if ( DO_SINUSOID ) { Serial.print( "Drive_frequency (" ); Serial.print( drive_frequency ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float drive_frequency_typed = Serial.parseFloat(); if ( drive_frequency_typed == 0 ) drive_frequency_typed = drive_frequency; Serial.print( "You typed: " ); Serial.println( drive_frequency_typed ); if ( drive_frequency_typed < -100 || drive_frequency_typed > 100 ) { Serial.println( "Bad value: Start over" ); return; } drive_frequency = drive_frequency_typed; freq_2PI_over_1000 = drive_frequency*2*PI/1000.0; desired_angle_amplitude = 2*PI; desired_velocity_amplitude = 4*PI*PI*drive_frequency; } if ( DO_MIN_JERK ) { Serial.print( "Movement_duration (" ); Serial.print( movement_duration ); Serial.println( ")?" ); Serial.setTimeout( 100000 ); // 10 seconds float movement_duration_typed = Serial.parseFloat(); if ( movement_duration_typed == 0 ) movement_duration_typed = movement_duration; Serial.print( "You typed: " ); Serial.println( movement_duration_typed ); if ( movement_duration_typed < 0.01 || movement_duration_typed > 10 ) { Serial.println( "Bad value: Start over" ); return; } movement_duration = movement_duration_typed; one_over_movement_duration = 1.0/movement_duration; } Encoder_init( &left_angle, &right_angle ); // set wheel encoders to zero run_balancer( launch_duration, wheel_angle_gain, wheel_angular_velocity_gain, body_angle_gain, body_angular_velocity_gain, past_command_gain, 0.0, RUN_DURATION, PLEASE_COLLECT_DATA ); Serial.println( "Stopped!" ); go = false; Serial.print( "servo_late: " ); Serial.println( servo_late ); Serial.print( "max_servo_late: " ); Serial.println( max_servo_late ); Serial.print( "Average encoder diff^2: " ); Serial.println( checks[ENCODER_DIFFS_SUM2]/check_counter ); Serial.print( "Average Y accelerometer reading: " ); Serial.println( checks[ACCELEROMETER_SUM]/check_counter ); Serial.print( "Average X gyro reading: " ); Serial.println( checks[GYRO_SUM]/check_counter ); } /**********************************************************************/