void setup()
{
pinMode(throttle_pin, INPUT);
pinMode(leftright_pin, INPUT);
pinMode(forwardback_pin, INPUT);
pinMode(rudder_pin, INPUT);
pinMode(acceldata_pin, INPUT);
pinMode(accelpower_pin, OUTPUT);
pinMode(led_pin, OUTPUT);
pinMode(motor1_pin, OUTPUT);
pinMode(motor2_pin, OUTPUT);
digitalWrite(led_pin, HIGH); //turn on signal LED before timers so it comes on immediately
digitalWrite(accelpower_pin, HIGH); //turn on power for accel (if ( accel is connected to chip for power)
//setup interrupts for RX pins
PCattachinterrupt(throttle_pin, throttle_change);
PCattachinterrupt(leftright_pin, leftright_change);
PCattachinterrupt(forwardback_pin, forwardback_change);
PCattachinterrupt(rudder_pin, rudder_change);
SetupTimer1(); //fire up timer1 (2 bytes) - accessed via TCNT1 variable
configmode = 0; //we're not in configmode to start
motors_off(); //make sure those motors are off...
throttle = 0; //make sure throttle is off at boot
//flash LED on boot
for (x = 1; x <= 20; x++)
{
digitalWrite(led_pin, !digitalRead(led_pin));
delay(20);
}
/*
if ( Eprom_check = 555 )
{ //load config data from eprom if ( eprom_check was set to 555
tracking_comp = tracking_comp_store;
if ( load_heading == 1 ) led_adjust = led_adjust_store;
if ( load_heading == 1 ) led_adjust_backup = led_adjust_store;
base_accel = base_accel_store;
}
*/
}
//sets up all the pins and interrupts
void setup(void)
{
adc_init(); //init the ADC...
set_throttle_pin_as_input();
set_leftright_pin_as_input();
set_forwardback_pin_as_input();
set_accel_data_pin_as_input();
set_accelpower_pin_as_output();
set_accelpower_pin_on(); //turn on power for accel (accel is connected to chip for power)
set_led_pin_as_output();
set_motor1_pin_as_output();
set_motor2_pin_as_output();
set_led_on(); //turn on signal LED before timers so it comes on immediately
//enable pin change interrupt - any changes on PORTB trigger interrupt
PCMSK0 = 0xFF;
PCICR = 1<<PCIE0;
SetupTimer1(); //fire up timer1 (2 bytes) - accessed via TCNT1 variable
motors_off(); //make sure those motors are off...
//flash LED on boot
for (x = 1; x <= 10; x++)
{
toggle_led();
_delay_ms(200);
}
throttle = 0; //make sure throttle is off at boot
}
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);
}
}
void evalOpcode(unsigned char opcode) {
int i;
int16 opr1, opr2, opr3;
unsigned int16 genPurpose = 0;
if (opcode & 0b10000000) {
genPurpose = gblMemPtr + 2;
gblMemPtr = ((((unsigned int16)opcode & 0b00111111) << 8) + 2*fetchNextOpcode());
opr1 = fetchNextOpcode();
if (opcode & 0b01000000) {
for (i = 0; i < (opr1 + 3); i++) {
inputPop();
}
}
for (i = 0; i < opr1; i++) {
inputPush (stkPop());
}
inputPush (gblStkPtr);
inputPush (genPurpose);
inputPush(gblInputStkPtr - (opr1 + 2));
return;
}
switch (opcode) {
case CODE_END:
gblLogoIsRunning = 0;
output_low (RUN_LED);
// clear thes variables just in case.
gblLoopAddress = 0;
gblRepeatCount = 0;
break;
case NUM8:
int8 Hi3 = fetchNextOpcode();
stkPush(Hi3);
break;
case NUM16:
int8 Hi2 = fetchNextOpcode();
int8 Lo2 = fetchNextOpcode();
unsigned int16 teste4 = Hi2 << 8;
teste4 += Lo2;
stkPush (teste4);
break;
case LIST:
stkPush(gblMemPtr + 2); //incremento de 2 em 2
gblMemPtr += (2 * fetchNextOpcode());
break;
case EOL:
genPurpose = stkPop();
if (genPurpose > gblMemPtr) {
//printf(usb_cdc_putc,"genPurpose >>>> gblMemPtr");
gblMemPtr = genPurpose;
} else {
//printf(usb_cdc_putc,"genPurpose <<<< gblMemPtr");
gblMemPtr = genPurpose;
gblRepeatCount = stkPop(); // repeat count
//printf(usb_cdc_putc," gblRepeatCount %Lu \n",genPurpose);
if (gblRepeatCount > 1)
gblRepeatCount--;
if (gblRepeatCount != 1) {
stkPush (gblRepeatCount);
stkPush (gblMemPtr);
}
}
break;
case EOLR:
if (stkPop()) { // if condition is true
stkPop(); // throw away the loop address
gblMemPtr = stkPop(); // fetch the next command address
} else { // if condition if false -> keep on looping.
gblMemPtr = stkPop();
stkPush (gblMemPtr);
delay_ms(5); // this prevents the waituntil loop to execute too rapidly
// which has proven to cause some problems when reading sensor values.
}
break;
case LTHING:
genPurpose = 2 * fetchNextOpcode(); // index of the input variable
opr1 = inputPop(); // base address in the input stack
inputPush(opr1); // push the base address back to the stack.
stkPush (gblInputStack[opr1 + genPurpose]);
break;
case STOP:
case OUTPUT:
if (opcode == OUTPUT){
genPurpose = stkPop(); // this is the output value
}
opr1 = inputPop(); // this is the proc-input stack base address
gblMemPtr = inputPop(); // this is the return address
opr2 = inputPop(); // this is the data stack index;
// remove any remaining data that belongs to the current procedure from the data stack
// Usually this is important for the STOP opcode.
while (gblStkPtr > opr2){
stkPop();
}
// remove the procedure inputs from the input stack
while (gblInputStkPtr > opr1){
inputPop();
}
// Output will push the output to the stack
if (opcode == OUTPUT){
stkPush (genPurpose);
}
break;
case REPEAT:
gblLoopAddress = stkPop();
gblRepeatCount = stkPop();
// these will be poped by EOL
stkPush (gblMemPtr); // address after repeat is complete
if (gblRepeatCount > 1) {
stkPush (gblRepeatCount);
stkPush (gblLoopAddress); // address while still repeating
gblMemPtr = gblLoopAddress;
} else if (gblRepeatCount == 1) {
gblMemPtr = gblLoopAddress;
} else { // if loop count = 0
gblMemPtr = stkPop();
}
break;
case COND_IF:
opr1 = stkPop(); // if true pointer address
opr2 = stkPop(); // condition
if (opr2) {
stkPush(gblMemPtr);
gblMemPtr = opr1;
}
break;
case COND_IFELSE:
opr1 = stkPop(); // if false pointer address
opr2 = stkPop(); // if true pointer address
opr3 = stkPop(); // condition
stkPush(gblMemPtr);
if (opr3) {
gblMemPtr = opr2;
} else {
gblMemPtr = opr1;
}
break;
case BEEP:
beep();
break;
case WAITUNTIL:
gblLoopAddress = stkPop();
// these will be poped by EOLR
stkPush(gblMemPtr); // address after repeat is complete
stkPush (gblLoopAddress); // address while still repeating
gblMemPtr = gblLoopAddress;
break;
case LOOP:
gblLoopAddress = stkPop(); // the begining of loop
gblRepeatCount = 0; // disable this counter (loop forever)
stkPush(0); // this distinguishes LOOP from Repeat. (see EOL)
stkPush(gblLoopAddress); // push loop address back into the stack
// so that EOL will loop
gblMemPtr = gblLoopAddress;
break;
case WAIT:
gblWaitCounter = stkPop() * 4;
break;
case TIMER:
stkPush(gblTimer); // gblTimer increases every 1ms. See in RTCC interrupt
break;
case RESETT:
gblTimer = 0;
break;
case SEND:
genPurpose = stkPop();
break;
case IR:
stkPush (gblMostRecentlyReceivedByte);
gblNewByteHasArrivedFlag = 0;
break;
case NEWIR:
stkPush (gblNewByteHasArrivedFlag);
break;
case RANDOM:
stkPush (rand());
break;
case OP_PLUS:
case OP_MINUS:
case OP_MULTIPLY:
case OP_DIVISION:
case OP_REMAINDER:
case OP_EQUAL:
case OP_GREATER:
case OP_LESS:
case OP_AND:
case OP_OR:
case OP_XOR:
opr2=stkPop(); // second operand
opr1=stkPop();// first operand
switch (opcode) {
case OP_PLUS:
opr1+=opr2;
break;
case OP_MINUS:
opr1-=opr2;
break;
case OP_MULTIPLY:
opr1*=opr2;
break;
case OP_DIVISION:
opr1/=opr2;
break;
case OP_REMAINDER:
opr1%=opr2;
break;
case OP_EQUAL:
opr1=(opr1==opr2);
break;
case OP_GREATER:
opr1=(opr1>opr2);
break;
case OP_LESS:
opr1=(opr1<opr2);
break;
case OP_AND:
opr1=(opr1&&opr2);
break;
case OP_OR:
opr1=(opr1||opr2);
break;
case OP_XOR:
opr1=(opr1^opr2);
break;
};
stkPush(opr1);
break;
case OP_NOT:
stkPush(!stkPop());
break;
case SETGLOBAL:
genPurpose = stkPop();// this is the value
globalVariables[stkPop()] = genPurpose;
break;
case GETGLOBAL:
stkPush(globalVariables[stkPop()]);
break;
case ASET:
opr2 = stkPop();// this is the value to be stored
opr1 = stkPop() * 2;// this is the array index. Each entry is two bytes wide.
genPurpose = ARRAY_BASE_ADDRESS + stkPop();// this is the base address of the array.
flashSetWordAddress(genPurpose + opr1);
flashWrite(opr2);
break;
case AGET:
opr1 = stkPop() * 2;// this is the array index. Each entry is two bytes wide.
genPurpose = ARRAY_BASE_ADDRESS + stkPop();// this is the base address of the array.
opr2 = read_program_eeprom(genPurpose + opr1);
stkPush(opr2);
break;
case RECORD:
genPurpose = stkPop();
// PCM parts (14 bit PICs like the 16F877) uses an external EEPROM for data Logging storage
flashSetWordAddress(RECORD_BASE_ADDRESS + gblRecordPtr++);
flashWrite(genPurpose);
// save current record pointer location
flashSetWordAddress(MEM_PTR_LOG_BASE_ADDRESS);
flashWrite(gblRecordPtr);
break;
case RECALL:
genPurpose = read_program_eeprom(RECORD_BASE_ADDRESS + gblRecordPtr++);
stkPush(genPurpose);
break;
case RESETDP:
gblRecordPtr = 0;
break;
case SETDP:
gblRecordPtr = stkPop() * 2;
break;
case ERASE:
opr1 = stkPop() * 2;
for (genPurpose=0; genPurpose<opr1; genPurpose++) {
flashSetWordAddress(RECORD_BASE_ADDRESS + genPurpose);
flashWrite(0);
}
gblRecordPtr = 0;
break;
case OPC_MOTORS_OFF:
motors_off();
break;
case OPC_MOTORS_THATWAY:
motors_that_way();
break;
case OPC_MOTORS_THISWAY:
motors_this_way();
break;
case OPC_MOTORS_REVERT:
motors_reverse();
break;
case OPC_MOTORS_ON_FOR:
case OPC_MOTORS_ON:
motors_on();
if (opcode == OPC_MOTORS_ON_FOR) {
set_on_for(stkPop()*4);
}
break;
case OPC_MOTORS_POWER:
motors_power(stkPop());
break;
case OPC_MOTORS_ANGLE:
motors_angle(stkPop());
break;
case OPC_ACTIVATE_MOTORS:
mtrsActive = stkPop();
break;
case REALLY_STOP:
start_stop_logo_machine = 1;
break;
case EB:
stkPush(read_program_eeprom(stkPop()));
break;
case DB:
opr1 = stkPop();
opr2 = stkPop();
flashSetWordAddress(opr2);
flashWrite(opr1);
break;
case LOW_BYTE:
stkPush(stkPop() & 0xff);
break;
case HIGH_BYTE:
stkPush(stkPop() >> 8);
break;
case SENSOR1:
case SENSOR2:
case SENSOR3:
case SENSOR4:
case SENSOR5:
case SENSOR6:
case SENSOR7:
case SENSOR8:
if (opcode < SENSOR3) {
i = opcode - SENSOR1;
} else {
i = opcode - SENSOR3 + 2;
}
stkPush(readSensor(i));
break;
case SWITCH1:
case SWITCH2:
case SWITCH3:
case SWITCH4:
case SWITCH5:
case SWITCH6:
case SWITCH7:
case SWITCH8:
if (opcode < SWITCH3) {
i = opcode - SWITCH1;
} else {
i = opcode - SWITCH3 + 2;
}
stkPush(readSensor(i)>>9);
break;
case ULED_ON:
output_high(USER_LED);
break;
case ULED_OFF:
output_low(USER_LED);
break;
case CL_I2C_START:
i2c_start();
break;
case CL_I2C_STOP:
i2c_stop();
break;
case CL_I2C_WRITE:
i2c_write(stkPop());
break;
case CL_I2C_READ:
stkPush(i2c_read(stkPop()));
break;
default:
start_stop_logo_machine = 1;
break;
};
}
