Esempio n. 1
0
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);

  }
}
Esempio n. 2
0
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();                                             
	}
	
	

}
Esempio n. 3
0
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);
  }

}