int main()
{
DDRB |= _BV(PB0);
DDRD = 0x00;
setup_motors();
int change_pwm = 0;
int prev_error = 0;
int error = 0;
int p = 0,i = 0,d = 0;
while(1) {
prev_error = error;
error = compute_error();
if(check_all()) {
for(int x = 0; x < 4; x++) {
errors[x] = errors[x+1];
}
errors[4] = error;
}
toggle_led();
if(!check_all()) {
int r_count = 0, l_count = 0;
for(int i = 0; i < 5; i++) {
if(errors[i] < 0)
l_count++;
if(errors[i] > 0)
r_count++;
}
if(l_count > r_count)
motors_left();
else
motors_right();
OCR1A = 255;
OCR1B = 255;
}
else {
p = error;
change_pwm = kp*p;
int right = tp + change_pwm;
int left = tp - change_pwm;
if(right > 255) right = 255;
if(left > 255) left = 255;
motors_straight();
OCR1A = right;
OCR1B = left;
}
_delay_ms(5);
}
}
void loop(void)
{
//if throttle is lower than throttle_low - or is over 100 beyond throttle_high - bot stays powered down
sei(); //enable interrupts
while ( throttle < throttle_low || throttle > (throttle_high + 100))
{
motors_off();
//interrupt blinking if stick isn't centered (helps to verify TX is working)
if ( leftright > heading_leftthresh ) _delay_ms(200);
if ( leftright < heading_rightthresh ) _delay_ms(200);
//sit there and flash LED
toggle_led();
_delay_ms(50);
}
cli(); //disable interrupts - bad things seem to happen if the RC interrupts get triggered while doing math...
//Are we going forward or backwards?
if ( forwardback > forwardback_forwardthresh) forward = 1; else forward = 0;
if ( forwardback < forwardback_backthresh) backward = 1; else backward = 0;
flashy_led = 0;
accel_raw_data = read_adc(); //get accel data
accel_read = accel_raw_data; //move it over to single in case we want to do floating point
accel_read = accel_read - base_accel; //compensate for base (2.5v) level
g = accel_read * g_per_adc_increment; //convert to g's
rpm = g * 89445; //calculate RPM from g's - derived from "G = 0.00001118 * r * RPM^2"
rpm = rpm / radius;
rpm = pow(rpm, .5);
periodms = rpm / 60; //convert RPM to duration of each spin in milliseconds
if (periodms == 0) periodms = 1; //must prevent any possible division by zero!!!
periodms = 1 / periodms;
periodms = periodms * 1000;
periodms = periodms * tracking_comp; //compensate with user-set tracking adjustment
alternate_motor_cycle = !alternate_motor_cycle; //alternates alternate_motor_cycle - used to balance spin
if ( forward == 1 ) periodms = periodms * forward_comp; //extra compensation if going forward
if ( backward == 1 ) periodms = periodms * backward_comp; //extra compensation if going backward
total_fudge_factor = fudge_factor + (fudge_factor_per_ms * periodms);
periodms = periodms - total_fudge_factor; //compensate for time spent doing reads / calculations
delaytime_single = periodms / 2; //sets period in MS for each half of spin
//converts throttle reading from remote into percentage (400 = low, 560 = high)
throttle_percent = (throttle - throttle_low) + 40; //this gives a range from about 20% to 100%
throttle_percent = throttle_percent / 2;
if ( throttle_percent > 100 ) throttle_percent = 100; //don't got over 100%
//calculates + modifies changes to heading based on input from transmitter
steering_multiplier = heading_center - leftright;
steering_multiplier = steering_multiplier * turn_speed;
steering_multiplier = 1 - steering_multiplier; //starts with 1 as a base value (ie - if it was 0.0 it becomes 1.0 - so there's no change in heading)
delaytime_single = delaytime_single * steering_multiplier;
delaytime_long = delaytime_single;
digitdif = delaytime_single - delaytime_long;
//code that waits X microseconds to compensate for integer math
digitdif = digitdif * 10; //converts digit dif to 50's of microseconds
//caps on timing if going too slow or fast
if ( delaytime_long > 500) delaytime_long = 500;
if ( delaytime_long < 5) delaytime_long = 5;
periodms = delaytime_long * 2; //we re-use periodms (full cycle length) for LED calculations - so it needs to be updated with any timing adjustments
periodms_long = periodms;
//set heading beacon size and location
led_on = periodms * led_adjust;
led_on = led_on / 100;
led_off = periodms / 3; //led signal is 33% of circle
led_off = led_off + led_on;
if (led_off >= periodms_long ) //if led_off is "later" or at end of cycle - shift led_off behind by one cycle
{
led_off = led_off - periodms_long;
}
if ( led_off < 1 ) led_off = led_off + periodms_long;
if ( g > max_g && throttle_percent > 20 ) throttle_percent = 20; //if we're over max RPM for translation - reduce throttle
//throttling
full_power_spin = 0;
if ( rpm < min_rpm ) full_power_spin = 1; //if we're under the minimum RPM for translation - do the full power spin!
if ( rpm > max_allowed_rpm ) throttle_percent = 6; //if we're over max RPM for translation - reduce power
//if throttle is at or over 50% throttle - adjust time spent in braking
if ( throttle_percent > 50 )
{
flashy_led = 1; //flash the LED to indicate we're in fast mode
braking_length = delaytime_long * 25;
braking_length = braking_length / throttle_percent;
begin_brake = delaytime_long / 2;
begin_brake = begin_brake - braking_length;
end_brake = delaytime_long / 2;
end_brake = end_brake + braking_length;
if ( begin_brake < 1 ) begin_brake = 1; //make sure begin_brake isn't getting set to 0
power_kill_part1 = 0; //power_kill not used if throttle over 50%
power_kill_part2 = delaytime_long;
}
if ( throttle_percent <= 50 ) //if throttle under 50% - kill the motors for a portion of each spin
{
begin_brake = 1;
end_brake = delaytime_long;
power_kill_length = 50 - throttle_percent; //set time in each cycle to cut power (throttling)
power_kill_length = power_kill_length * delaytime_long;
power_kill_length = power_kill_length / 150;
power_kill_part1 = power_kill_length;
power_kill_part2 = delaytime_long - power_kill_length;
}
if ( full_power_spin == 1 ) //if we're actually doing full power this spin (no translation) - ignore any calculations / reset variables
{
end_brake = 1;
begin_brake = 0;
power_kill_part1 = 0;
power_kill_part2 = delaytime_long;
}
sei(); //enable interrupts - out of all the critical stuff
led_ref = 0;
//Do translational drift driving
//Cycle 1 ("front" 180 degrees of spin)
for (x = 1; x <= delaytime_long; x++)
{
motors_left(); //start off under full power
led_ref = led_ref + 1;
if ( x >= begin_brake && x < end_brake ) //switch to single motor as soon as entering braking cycle
{
//if sitting still
if ( forward == 0 && backward == 0 )
{
if ( alternate_motor_cycle == 0 ) motor1_on(); //alternates which motor is used each cycle if ( sitting still
if ( alternate_motor_cycle == 1 ) motor2_on(); //this prevents unwanted "translation" due to any imbalances
}
//if ( going forward / back set motors appropriately (this is "where it happens")
if ( forward == 1 ) motor1_on();
if ( backward == 1 ) motor2_on();
}
if ( x >= end_brake ) motors_left(); //if we hit end of brake cycle - go to full power
if ( x < power_kill_part1 ) motors_off(); //if th rottle is less that 100% - kill power at appropriate time
if ( x > power_kill_part2 ) motors_off(); //if throttle is less that 100% - kill power at appropriate time
if ( led_ref == led_on ) led_is_on_now = 1; //turn on heading led
if ( led_ref == led_off ) led_is_on_now = 0; //turn off heading led
if ( led_is_on_now == 1 )
{
//flash the LED if we're in flashy mode - otherwise it's just on
if ( flashy_led == 1 ) toggle_led(); else set_led_on();
}
if ( led_is_on_now == 0 ) set_led_off();
_delay_ms(1);
}
for (delay_loop = 1; delay_loop < digitdif; delay_loop++)
{
//in theory we should wait 25 microseconds - but I'm assuming the overhead takes up 1us
wait24us();
}
//Cycle 2 (back 180 degrees of spin) - same as above except motors are reversed (there are some moderately-good reasons this isn't a subroutine)
for (x = 1; x <= delaytime_long; x++)
{
motors_left(); //start off under full power
led_ref = led_ref + 1;
if ( x >= begin_brake && x < end_brake ) //switch to single motor as soon as entering braking cycle
{
//if sitting still
if ( forward == 0 && backward == 0 )
{
if ( alternate_motor_cycle == 0 ) motor2_on(); //alternates which motor is used each cycle if ( sitting still
if ( alternate_motor_cycle == 1 ) motor1_on(); //this prevents unwanted "translation" due to any imbalances
}
//if ( going forward / back set motors appropriately (this is "where it happens")
if ( forward == 1 ) motor2_on();
if ( backward == 1 ) motor1_on();
}
if ( x >= end_brake ) motors_left(); //if we hit end of brake cycle - go to full power
if ( x < power_kill_part1 ) motors_off(); //if throttle is less that 100% - kill power at appropriate time
if ( x > power_kill_part2 ) motors_off(); //if throttle is less that 100% - kill power at appropriate time
if ( led_ref == led_on ) led_is_on_now = 1; //turn on heading led
if ( led_ref == led_off ) led_is_on_now = 0; //turn off heading led
if ( led_is_on_now == 1 )
{
//flash the LED if we're in flashy mode - otherwise it's just on
if ( flashy_led == 1 ) toggle_led(); else set_led_on();
}
if ( led_is_on_now == 0 ) set_led_off();
_delay_ms(1);
}
for (delay_loop = 1; delay_loop < digitdif; delay_loop++)
{
//in theory we should wait 25 microseconds - but I'm assuming the overhead takes up 1us
wait24us();
}
}
void loop()
{
//if throttle is lower than throttle_low - or is over 100 beyond throttle_high - bot stays powered down
while ( throttle < throttle_low || throttle > (throttle_high + 100))
{
motors_off();
//interrupt blinking if ( stick isn//t centered
//used to help debug if ( center is off
if ( leftright > heading_rightthresh ) delay(200);
if ( leftright < heading_leftthresh ) delay(200);
//if ( stick is pulled back (and not in configmode) - flash out highest RPM
if ( forwardback < forwardback_backthresh && configmode == 0 )
{
digitalWrite(led_pin, LOW);
delay(500);
for (y = 1; y <= max_rpm; y = y + 100)
{
digitalWrite(led_pin, HIGH);
delay(100);
digitalWrite(led_pin, LOW);
delay(300);
if ( throttle >= throttle_low ) y = max_rpm; //abort if throttle gets pushed up
}
if ( throttle < throttle_low ) delay(2000); //only wait if throttle is still low
}
//if ( stick is upper right - ) { toggle configmode
if ( forwardback > forwardback_forwardthresh && leftright > rudder_leftthresh )
{
//wait a bit to make sure stick is being held...
delay(1400);
if ( forwardback > forwardback_forwardthresh && leftright > rudder_leftthresh )
{
if ( configmode == 0 )
{
configmode = 1;
//assign base_accel
accel_raw_data = analogRead(acceldata_pin);
base_accel = accel_raw_data;
}
else
{
configmode = 0;
/* //write out new data to ROM
tracking_comp_store = tracking_comp; //write out config data to ROM
led_adjust_store = led_adjust;
base_accel_store = base_accel;
Eprom_check = 555; //write out arbitrary value to validate tracking_comp was written out
*/
}
delay(500);
}
}
if ( forwardback < forwardback_backthresh && leftright < rudder_rightthresh && configmode == 1 )
{
////if ( stick is held to back left while ( in config mode
//reset heading adjustment
//wait a bit to make sure stick is being held...
delay(1400);
if ( forwardback < forwardback_backthresh && leftright < rudder_rightthresh && configmode == 1 )
{
tracking_comp = 1; //reset heading adjustment
for ( x = 1; x <= 70; x++)
{
digitalWrite(led_pin, !digitalRead(led_pin));
delay(20);
}
digitalWrite(led_pin, LOW);
delay(1000);
}
}
//sit there and flash LED
digitalWrite(led_pin, LOW);
delay(50);
if ( configmode == 1 )
{
for ( x = 1; x <=10; x++)
{
delay(30);
digitalWrite(led_pin, !digitalRead(led_pin));
}
digitalWrite(led_pin, LOW);
delay(150);
}
digitalWrite(led_pin, HIGH);
delay(50);
}
//safety
//if we haven't gotten the throttle reading in 5 cycles - set throttle to 0
got_throttle = got_throttle + 1;
if ( got_throttle > 100 ) got_throttle = 100;
if ( got_throttle > 5) throttle = 0;
//reset max RPM
max_rpm = 0;
cli(); //disable interrupts - bad things seem to happen if the RC interrupts get triggered while doing math...
//Are we going forward or backwards?
if ( forwardback > forwardback_forwardthresh) {
forward = 1;
}
else {
forward = 0;
}
if ( forwardback < forwardback_backthresh) {
backward = 1;
}
else {
backward = 0;
}
flashy_led = 0;
accel_raw_data = analogRead(acceldata_pin); //get accel data
accel_read = accel_raw_data; //move it over to single in case we want to do floating point
accel_read = accel_read - base_accel; //compensate for base (2.5v) level
g = accel_read * g_per_adc_increment; //convert to g's
rpm = 28.45 * radius; //calculate RPM from g's - rpm = (g/(28.45* radius ))^0.5 *1000
if (rpm == 0) rpm = 1; //must prevent any possible division by zero!!!
rpm = g / rpm;
rpm = pow(rpm, .5);
rpm = rpm * 1000;
if ( rpm > max_rpm ) max_rpm = rpm;
periodms = rpm / 60; //convert RPM to duration of each spin in milliseconds
if (periodms == 0) periodms = 1; //must prevent any possible division by zero!!!
periodms = 1 / periodms;
periodms = periodms * 1000;
periodms = periodms * tracking_comp; //compensate with user-set tracking adjustment
//test!
periodms = 30;
if ( alternate_motor_cycle == 1 ) {
alternate_motor_cycle = 2;
}
else {
alternate_motor_cycle = 1;
} //alternates alternate_motor_cycle - used to balance spin
//strafing
if ( strafing_is_off == 0 ) //don't do strafing stuff if it's off
{
if ( no_led == 1 ) //skip straffing for one cycle if we had a change last time (setting no_led to 1)
{
no_led = 0;
}
else
{
no_led = 0; //make sure LED is recalc on unless we decide to turn it off
led_adjust = led_adjust_backup;
if ( rudder > rudder_leftthresh )
{
if ( left_strafe == 0 )
{
periodms = periodms * 1.25; //one time adjustment to turn 90 degrees
no_led = 1; // led recalc off this cycle - otherwise will look glitchy
}
led_adjust = led_adjust - 25;
left_strafe = 1;
forward = 1;
backward = 0;
}
else
{
if ( left_strafe == 1 )
{
periodms = periodms * .75; //correct for prior straffing
no_led = 1; // led recalc off this cycle - otherwise will look glitchy
}
left_strafe = 0;
}
if ( rudder < rudder_rightthresh )
{
if ( right_strafe == 0 )
{
periodms = periodms * .75; //one time adjustment to turn 90 degrees
no_led = 1; // led recalc off this cycle - otherwise will look glitchy
}
led_adjust = led_adjust + 25;
right_strafe = 1;
forward = 1;
backward = 0;
}
else
{
if ( right_strafe == 1 )
{
periodms = periodms * 1.25; //correct for prior straffing
no_led = 1; // led recalc off this cycle - otherwise will look glitchy
}
right_strafe = 0;
}
}
}
if ( forward == 1 ) periodms = periodms * forward_comp; //extra compensation if going forward
if ( backward == 1 ) periodms = periodms * backward_comp; //extra compensation if going backward
periodms = periodms - fudge_factor; //compensate for time spent doing reads / calculations
//make sure led_adjust didn't get set above or below limits
if ( led_adjust < 1 ) led_adjust = led_adjust + 100;
if ( led_adjust > 100 ) led_adjust = led_adjust - 100;
delaytime_single = periodms / 2; //sets period in MS for each half of spin
//converts throttle reading from remote into percentage (400 = low, 560 = high)
throttle_percent = (throttle - throttle_low) + 40; //this gives a range from about 20% to 100%
throttle_percent = throttle_percent / 2;
if ( throttle_percent > 100 ) throttle_percent = 100; //don't got over 100%
//driver moves stick left and right until the bot tracks correctly
//data is written into eprom next time the robot spins down
in_tracking_adjust = 0;
//tracking adjustment - if throttle is neither forward or back and throttle is under 50% go into tracking adjustment mode
if ( configmode == 1 && forward == 0 && backward == 0 && throttle_percent < 50 )
{
in_tracking_adjust = 1;
flashy_led = 1; //to indicate heading adjust most - LED flashing is on
if ( leftright > heading_rightthresh || leftright < heading_leftthresh )
{
flashy_led = 0; //turn off flashing to show is actively being adjusted
add_delay = heading_center - leftright;
add_delay = add_delay * delaytime_single;
// add_delay = pow(add_delay , exponential); //disabled - need to fix problem with negative values
add_delay = add_delay / 1400000; //(was 14000000)
tracking_comp = tracking_comp + add_delay;
}
}
//adjust led direction if throttle is over 50% and in configmode + not going back / forward
if ( configmode == 1 && forward == 0 && backward == 0 && throttle_percent >= 50 )
{
in_tracking_adjust = 1;
flashy_led = 0; //to indicate LED adjust most - LED flashing is off
if ( leftright < heading_leftthresh )
{
led_adjust = led_adjust + 1;
flashy_led = 1; //turn on flashing to indicate change
}
if ( leftright > heading_rightthresh )
{
led_adjust = led_adjust - 1;
flashy_led = 1; //turn on flashing to indicate change
}
if ( led_adjust < 1 ) led_adjust = 100; //"wrap" around when adjusting LED direction
if ( led_adjust > 100 ) led_adjust = 1;
backward = 0; //don//t really go backwards
led_adjust_backup = led_adjust;
}
if ( in_tracking_adjust == 0 )
{
//don't do normal heading adjustments if we're doing tracking adjustments
//normal drive heading change
//this code adds or subtracts a percentage of delaytime based on the heading data from the remote
//don't do if in configmode
if ( leftright > heading_rightthresh || leftright < heading_leftthresh )
{
add_delay = heading_center - leftright;
add_delay = add_delay * delaytime_single;
//add_delay = Pow(add_delay , exponential); //disabled - need to fix problem with negative values
add_delay = add_delay / 700; //(was 7700)
delaytime_single = delaytime_single + add_delay;
}
}
if ( configmode == 1) throttle_percent = 50; //ignore throttle if in configmode
delaytime_long = delaytime_single;
digitdif = delaytime_single - delaytime_long;
//code that waits X microseconds to compensate for integer math
digitdif = digitdif * 10; //converts digit dif ( to 50's of microseconds
for (delay_loop =1; delay_loop <= digitdif; delay_loop++)
{
wait25us();
}
//caps on timing if going too slow or fast
if ( delaytime_long > 500) delaytime_long = 500;
if ( delaytime_long < 5) delaytime_long = 5;
periodms = delaytime_long * 2; //we re-use periodms (full cycle length) for LED calculations - so it needs to be updated with any timing adjustments
periodms_long = periodms;
//set heading beacon size and location
led_on = periodms * led_adjust;
led_on = led_on / 100;
led_off = periodms / 3; //led signal is 33% of circle
led_off = led_off + led_on;
if (led_off >= periodms_long ) //if led_off is "later" or at end of cycle - shift led_off behind by one cycle
{
led_off = led_off - periodms_long;
}
if ( led_off < 1 ) led_off = led_off + periodms_long;
tail_start = periodms_long * 17; //code to calculate position of LED tail
tail_start = tail_start / 60;
tail_start = tail_start + led_off;
tail_end = periodms_long * 6;
tail_end = tail_end / 60;
tail_end = tail_end + tail_start;
if ( tail_start >= periodms_long )
{
tail_start = tail_start - periodms_long;
}
if ( tail_end >= periodms_long )
{
tail_end = tail_end - periodms_long;
}
if ( g > max_g && throttle_percent > 20 ) throttle_percent = 20; //if we're over max RPM for translation - reduce throttle
//throttling
if ( throttle_percent > throttle_percent_max ) throttle_percent = throttle_percent_max;
if ( throttle_percent < throttle_percent_min ) throttle_percent = throttle_percent_min;
full_power_spin = 0;
cut_power = 0;
if ( rpm < min_rpm ) full_power_spin = 1; //if we're under the minimum RPM for translation - do the full power spin!
if ( rpm > max_allowed_rpm ) throttle_percent = 6; //if we're over max RPM for translation - reduce power
//if throttle is at or over 50% throttle - adjust time spent in braking
if ( throttle_percent > 50 )
{
flashy_led = 1; //flash the LED to indicate we're in fast mode
braking_length = delaytime_long * 25; //original
braking_length = braking_length / throttle_percent;
begin_brake = delaytime_long / 2;
begin_brake = begin_brake - braking_length;
end_brake = delaytime_long / 2;
end_brake = end_brake + braking_length;
if ( begin_brake < 1 )
{
begin_brake = 1; //make sure begin_brake isn//t getting set to 0
power_kill_part1 = 0; //power_kill not used if throttle over 50%
power_kill_part2 = delaytime_long;
}
}
if ( throttle_percent <= 50 ) //if throttle under 50% - kill the motors for a portion of each spin
{
begin_brake = 1;
end_brake = delaytime_long;
power_kill_length = 50 - throttle_percent; //set time in each cycle to cut power (throttling)
power_kill_length = power_kill_length * delaytime_long;
power_kill_length = power_kill_length / 150;
power_kill_part1 = power_kill_length;
power_kill_part2 = delaytime_long - power_kill_length;
}
if ( no_led == 1 ) //kill tail if initiating strafe
{
tail_start = 0;
tail_end = 0;
}
sei(); //enable interrupts - out of all the critical stuff
if ( full_power_spin == 1 ) //reset variables for full power spin
{
end_brake = 1;
begin_brake = 0;
power_kill_part1 = 0;
power_kill_part2 = delaytime_long;
}
//Do translational drift driving
//Cycle 1 (front 180 degrees of spin)
led_ref = 0;
for (x = 1; x <= delaytime_long; x++)
{
motors_left(); //start off under full power
led_ref = led_ref + 1;
if ( x >= begin_brake && x < end_brake ) //switch to single motor as soon as entering braking cycle
{
//if sitting still
if ( forward == 0 && backward == 0 )
{
if ( alternate_motor_cycle == 1 ) motor1_on(); //alternates which motor is used each cycle if ( sitting still
if ( alternate_motor_cycle == 2 ) motor2_on(); //this prevents unwanted "translation" due to any imbalances
}
//if ( going forward / back set motors appropriately (this is "where it happens")
if ( forward == 1 ) motor1_on();
if ( backward == 1 ) motor2_on();
}
if ( x >= end_brake ) motors_left; //if ( we hit end of brake cycle - go to full power
if ( x < power_kill_part1 ) motors_off; //if ( throttle is less that 100% - kill power at appropriate time
if ( x > power_kill_part2 ) motors_off; //if ( throttle is less that 100% - kill power at appropriate time
if ( cut_power == 1 ) motors_off; //if ( this is a no-power spin
if ( led_ref == led_on ) led_is_on_now = 1; //turn on heading led
if ( led_ref == led_off ) led_is_on_now = 0; //turn off heading led
if ( no_led == 1 ) led_is_on_now = 0;
if ( led_is_on_now == 1 )
{
if ( flashy_led == 1 ) {
digitalWrite(led_pin, !digitalRead(led_pin));
}
else {
digitalWrite(led_pin, HIGH);
}
}
if ( led_is_on_now == 0 ) digitalWrite(led_pin, LOW);
turn_on_tail;
// delay_1_ms;
delayMicroseconds(1250);
}
for (delay_loop = 1; delay_loop < digitdif; delay_loop++)
{ //extra delay to compensate for integer math (other half done earlier)
wait25us();
}
//Cycle 2 (back 180 degrees of spin) - pretty much everything works the same...
for (x = 1; x <= delaytime_long; x++) //each loop is 1ms (delaytime is length of 180 degrees of cycle)
{
motors_left; //start off under full power
led_ref = led_ref + 1;
if ( x >= begin_brake && x < end_brake ) { //switch to single motor as soon as entering braking cycle
if ( forward = 0 && backward == 0 ) //if ( sitting still
{
if ( dont_alternate_motors == 1 ) //swap motors once per rotation when not moving
{
if ( alternate_motor_cycle == 1 ) motor1_on;
if ( alternate_motor_cycle == 2 ) motor2_on;
else
if ( alternate_motor_cycle == 1 ) motor2_on; //alternates which motor is used each cycle if sitting still
if ( alternate_motor_cycle == 2 ) motor1_on; //this prevents unwanted "translation" due to any imbalances
} //if just using one wheel - this "pulses" it
}
//if ( going forward / back set motors appropriately (this is "where it happens")
if ( forward = 1 ) motor2_on;
if ( backward = 1 ) motor1_on;
}
if ( x >= end_brake ) motors_left; //if we hit end of brake cycle - go to full power
if ( x < power_kill_part1 ) motors_off; //if throttle is less that 100% - kill power at appropriate time
if ( x > power_kill_part2 ) motors_off; //if throttle is less that 100% - kill power at appropriate time
if ( cut_power == 1 ) motors_off; //if this is a no-power spin
if ( led_ref == led_on ) led_is_on_now = 1; //turn on heading led
if ( led_ref == led_off ) led_is_on_now = 0; //turn off heading led
if ( no_led == 1 ) led_is_on_now = 0;
if ( led_is_on_now == 1 )
{
if ( flashy_led == 1 ) {
digitalWrite(led_pin, !digitalRead(led_pin));
}
else {
digitalWrite(led_pin, HIGH);
}
}
if ( led_is_on_now == 0 ) digitalWrite(led_pin, LOW);
turn_on_tail;
// delay_1_ms;
delayMicroseconds(1250);
}
}