450 lines
16 KiB
C
450 lines
16 KiB
C
//Copyright (c) 2012, vsluiter <info-at- hackvandedam.nl>
|
|
//
|
|
//Permission to use, copy, modify, and/or distribute this software for any
|
|
//purpose with or without fee is hereby granted, provided that the above
|
|
//copyright notice and this permission notice appear in all copies.
|
|
//
|
|
//THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
|
|
//WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
|
|
//AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
|
|
//INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS
|
|
//OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
|
|
//TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
|
//OF THIS SOFTWARE.
|
|
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
#include <avr/eeprom.h>
|
|
#include <avr/sleep.h>
|
|
#include <avr/power.h>
|
|
#include "hal.h"
|
|
#include "openpf.h"
|
|
|
|
uint8_t version[]="OpenPF-V0.0"; //was: 1.2
|
|
uint8_t pwmport, servoatrip, servobtrip;
|
|
uint16_t pwma,pwmb;
|
|
volatile uint8_t externalint = 0;
|
|
volatile uint8_t timerflag105us = 0;
|
|
volatile uint8_t ocr1_mask_a,ocr1_mask_b,ocr1_mask_both = 0xFF;
|
|
|
|
static void UpdateOutputValues(struct OpenPfRx_output * );
|
|
static void ResetPWMChannel(struct OpenPfRx_channel *);
|
|
static void ResetServoChannel(struct OpenPfRx_channel *);
|
|
|
|
void OpenPfRxdebug(void)
|
|
{
|
|
asm("nop");//LED_RED_TOGGLE;
|
|
}
|
|
/*
|
|
Code is meant for 8MHz operation
|
|
Lego PowerFunctions RC v1.2
|
|
*/
|
|
|
|
#define SERVO_OFFSET 18
|
|
#define SERVO_SAME_CHANNEL 1
|
|
#define SERVO_NEXT_CHANNEL 0
|
|
#define EEPROM_ADDRESS_CHANNEL (uint8_t *)0
|
|
#define EEPROM_ADDRESS_SERVO_CHANNEL_MODE (uint8_t *)1
|
|
|
|
ISR(EXTERNAL_INTERRUPT, ISR_NOBLOCK) //External Interrupt Handler
|
|
{
|
|
//DISABLE_IR_INT; //Disable pin change interrupt. Enabled in timer interrupt routine.
|
|
RESET_IR_TIMER;
|
|
externalint = 1;
|
|
}
|
|
|
|
ISR(TIMER_105US, ISR_NOBLOCK)
|
|
{
|
|
OpenPfRx105usState();
|
|
timerflag105us = 1;
|
|
}
|
|
|
|
ISR(WATCHDOG_vect)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
//Restore stop and start timer
|
|
ISR(PWMTIMER_PERIODSTART)
|
|
{
|
|
static uint8_t servoloopcount;
|
|
uint8_t motordriver_running;
|
|
STOP_PWM_TIMER;
|
|
motordriver_running = A_PORT & (A_C1|A_C2|B_C1|B_C2);
|
|
A_PORT = (A_PORT & (~(A_C1|A_C2|B_C1|B_C2))) | (pwmport & (A_C1|A_C2|B_C1|B_C2)) ; //clear / set pwm output bits
|
|
if(motordriver_running)
|
|
{
|
|
OCR1A = pwma;
|
|
OCR1B = pwmb;
|
|
}
|
|
else
|
|
{
|
|
OCR1A = pwma +20;
|
|
OCR1B = pwmb +20;
|
|
}
|
|
if(ocr1_mask_a == ocr1_mask_b)
|
|
{
|
|
ocr1_mask_both = ocr1_mask_a & ocr1_mask_b;
|
|
OCR1B = pwmb + 1; //always execute interrupt routine for PWMA first
|
|
}
|
|
else
|
|
{
|
|
ocr1_mask_both = 0xFF;
|
|
}
|
|
RESET_PWM_TIMER;
|
|
START_PWM_TIMER;
|
|
if(servoloopcount == 0)
|
|
{
|
|
SERVOAPINHI;
|
|
SERVOBPINHI;
|
|
}
|
|
if(servoloopcount == servoatrip)
|
|
SERVOAPINLO;
|
|
if(servoloopcount == servobtrip)
|
|
SERVOBPINLO;
|
|
servoloopcount++; //runs until 255, then wraps around.
|
|
}
|
|
|
|
ISR(PWMTIMER_PWMA_INTERRUPT)
|
|
{
|
|
A_PORT &= (ocr1_mask_a & ocr1_mask_both);
|
|
}
|
|
|
|
ISR(PWMTIMER_PWMB_INTERRUPT)
|
|
{
|
|
A_PORT &= (ocr1_mask_b & ocr1_mask_both);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
uint8_t channelnumber,servo_channel_mode;
|
|
struct OpenPfRx_channel channel_pwm;
|
|
struct OpenPfRx_channel channel_servo;
|
|
uint16_t legochannel[8] = {0,0,0,0,0,0,0,0}; //can be used to store retrieved data
|
|
IoInit();
|
|
while(BUTTON_PUSHED);
|
|
power_adc_disable();
|
|
power_usi_disable();
|
|
ocr1_mask_a = ocr1_mask_b = 0xFF;
|
|
eeprom_busy_wait();
|
|
servo_channel_mode = eeprom_read_byte(EEPROM_ADDRESS_SERVO_CHANNEL_MODE);
|
|
eeprom_busy_wait();
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_pwm, (eeprom_read_byte(EEPROM_ADDRESS_CHANNEL)&0x03) );
|
|
if(servo_channel_mode == SERVO_SAME_CHANNEL)
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo, (channel_pwm.channel_number)& 0x03 );
|
|
else
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo, (channel_pwm.channel_number+1)& 0x03 );
|
|
SetupExternalInterrupt(ExternalInterruptFalling);
|
|
ENABLE_IR_INT;
|
|
SetupPWMTimer();
|
|
ENABLE_PWMA_INTERRUPT;
|
|
ENABLE_PWMB_INTERRUPT;
|
|
servoatrip = servobtrip = SERVO_OFFSET; //reset servos to center position
|
|
Setup105usclock();
|
|
OpenPfRx_rx.newdata = 0;
|
|
sei(); //Enable interrupts
|
|
set_sleep_mode(SLEEP_MODE_PWR_DOWN);
|
|
//start of main loop
|
|
for(;;)
|
|
{
|
|
static uint16_t red_led_downcounter = 0;
|
|
uint8_t c_sreg;
|
|
if(red_led_downcounter == 0)
|
|
BICOLOR_GREEN;
|
|
else
|
|
red_led_downcounter--;
|
|
if(OpenPfRx_rx.newdata)
|
|
{
|
|
OpenPfRx_rx.newdata = 0;
|
|
if(OpenPfRxVerifyChecksum(OpenPfRx_rx.rxdata)) //if Checksum is OK continue processing data
|
|
{
|
|
BICOLOR_RED;
|
|
red_led_downcounter = 10000;
|
|
channelnumber = OpenPfRxGetChannelNumber(OpenPfRx_rx.rxdata); //Read channelnumber from received data
|
|
legochannel[channelnumber] = OpenPfRx_rx.rxdata;
|
|
if(channelnumber == channel_pwm.channel_number) //if data is for own channel
|
|
{
|
|
uint8_t a_mask = 0xff,b_mask = 0xff;
|
|
uint16_t a_pwm = 0xF0, b_pwm = 0xF0;
|
|
uint8_t enablepwma = 1, enablepwmb = 1;
|
|
uint8_t temp_var= ~(A_C1|A_C2|B_C1|B_C2);
|
|
channel_pwm.timeout = channel_pwm.timeout_limit; //reset timeout
|
|
OpenPfRxInterpreter((const uint16_t *)&legochannel[channelnumber] , &channel_pwm);
|
|
if(channel_pwm.A.output_mode == (OM_FWD) || channel_pwm.A.output_mode == (OM_BWD))
|
|
{
|
|
if(channel_pwm.A.pwmvalue == OpenPfRx_MIN_PWM_VALUE || channel_pwm.A.pwmvalue == OpenPfRx_MAX_PWM_VALUE)
|
|
{
|
|
enablepwma = 0;
|
|
}
|
|
}
|
|
else
|
|
enablepwma = 0; //OM_BRAKE, OM_BRAKE_THEN_FLOAT, OM_INDEPENDENT, OM_FLOAT
|
|
if(channel_pwm.B.output_mode == (OM_FWD) || channel_pwm.B.output_mode == (OM_BWD))
|
|
{
|
|
if(channel_pwm.B.pwmvalue == OpenPfRx_MIN_PWM_VALUE || channel_pwm.B.pwmvalue == OpenPfRx_MAX_PWM_VALUE)
|
|
{
|
|
enablepwmb = 0;
|
|
}
|
|
}
|
|
else
|
|
enablepwmb = 0; //OM_BRAKE, OM_BRAKE_THEN_FLOAT, OM_INDEPENDENT, OM_FLOAT
|
|
UpdateOutputValues(&channel_pwm.A); //Calculate new values for output A, C1 and C2
|
|
UpdateOutputValues(&channel_pwm.B); //Calculate new values for output B, C1 and C2
|
|
if(channel_pwm.B.C1)
|
|
temp_var |= B_C1;
|
|
if(channel_pwm.B.C2)
|
|
temp_var |= B_C2;
|
|
if(channel_pwm.A.C1)
|
|
temp_var |= A_C1;
|
|
if(channel_pwm.A.C2)
|
|
temp_var |= A_C2;
|
|
c_sreg = SREG;
|
|
if(enablepwma)
|
|
{
|
|
a_mask = ~(A_C1|A_C2);
|
|
a_pwm = (channel_pwm.A.pwmvalue) <<1;
|
|
}
|
|
else
|
|
{
|
|
a_mask = 0xff;
|
|
a_pwm = 0x30;
|
|
}
|
|
if(enablepwmb)
|
|
{
|
|
b_mask = ~(B_C1|B_C2);
|
|
b_pwm = (channel_pwm.B.pwmvalue) <<1;
|
|
}
|
|
else
|
|
{
|
|
b_mask = 0xFF;
|
|
b_pwm = 0x30;
|
|
}
|
|
cli();
|
|
//pwma = (uint16_t)(channel_pwm.A.pwmvalue) <<1;
|
|
//pwmb = (uint16_t)(channel_pwm.B.pwmvalue) <<1;
|
|
|
|
pwma = a_pwm;
|
|
pwmb = b_pwm;
|
|
ocr1_mask_a = a_mask;
|
|
ocr1_mask_b = b_mask;
|
|
pwmport = temp_var;
|
|
sei();
|
|
SREG = c_sreg;
|
|
}
|
|
if(channelnumber == channel_servo.channel_number)
|
|
{
|
|
channel_servo.timeout = channel_servo.timeout_limit; //reset timeout
|
|
OpenPfRxInterpreter((const uint16_t *)&legochannel[channelnumber] , &channel_servo);
|
|
|
|
if(channel_servo.A.output_mode == OM_FWD || channel_servo.A.output_mode == OM_BWD)
|
|
{
|
|
int8_t temp;
|
|
temp = (int8_t)channel_servo.A.pwmindex;
|
|
if(channel_servo.A.output_mode == OM_BWD)
|
|
temp = -temp;
|
|
servoatrip = SERVO_OFFSET + temp;
|
|
}
|
|
else
|
|
{
|
|
servoatrip = SERVO_OFFSET;
|
|
}
|
|
if(channel_servo.B.output_mode == OM_FWD || channel_servo.B.output_mode == OM_BWD)
|
|
{
|
|
int8_t temp;
|
|
temp = (int8_t)channel_servo.B.pwmindex;
|
|
if(channel_servo.B.output_mode == OM_BWD)
|
|
temp = -temp;
|
|
servobtrip = SERVO_OFFSET + temp;
|
|
}
|
|
else
|
|
{
|
|
servobtrip = SERVO_OFFSET;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if(externalint) //check if external interrupt has taken place
|
|
{
|
|
externalint = 0; //clear flag
|
|
OpenPfRxPinInterruptState(); //process counters / pin state to gather IR data
|
|
}
|
|
if(timerflag105us)
|
|
{
|
|
timerflag105us = 0;
|
|
channel_pwm.A.brakethenfloatcount++;
|
|
channel_pwm.B.brakethenfloatcount++;
|
|
if(channel_pwm.timeout)
|
|
--channel_pwm.timeout;
|
|
if(channel_servo.timeout)
|
|
--channel_servo.timeout;
|
|
}
|
|
if(channel_pwm.timeout == 0 && channel_pwm.timeout_action)
|
|
{
|
|
ResetPWMChannel(&channel_pwm);
|
|
//asm("nop");
|
|
}
|
|
if(channel_servo.timeout == 0 && channel_servo.timeout_action)
|
|
{
|
|
ResetServoChannel(&channel_servo);
|
|
}
|
|
if( (channel_pwm.A.output_mode == OM_BRAKE_THEN_FLOAT) && channel_pwm.A.brakethenfloatcount >= 2000)
|
|
{
|
|
channel_pwm.A.output_mode = OM_FLOAT;
|
|
channel_pwm.A.pwmindex = PWM_FLOAT;
|
|
pwmport &= ~(A_C1|A_C2);
|
|
ocr1_mask_a = 0xFF;
|
|
|
|
}
|
|
if( (channel_pwm.B.output_mode == OM_BRAKE_THEN_FLOAT) && channel_pwm.B.brakethenfloatcount >= 2000)
|
|
{
|
|
channel_pwm.B.output_mode = OM_FLOAT;
|
|
channel_pwm.B.pwmindex = PWM_FLOAT;
|
|
pwmport &= ~(B_C1|B_C2);
|
|
ocr1_mask_b = 0xFF;
|
|
}
|
|
if(BUTTON_PUSHED)
|
|
{
|
|
uint8_t temp_channel;
|
|
uint8_t push_8ms;
|
|
push_8ms = 0;
|
|
cli(); //disable interrupts
|
|
while(BUTTON_PUSHED) //wait until button is released to write new value
|
|
{
|
|
uint16_t tempvar;
|
|
for(tempvar = 0; tempvar < 65000; tempvar++);
|
|
//_delay_ms(100);
|
|
if(push_8ms <= 100)
|
|
++push_8ms;
|
|
}
|
|
if(push_8ms > 99) //button pressed more than a second
|
|
{
|
|
eeprom_busy_wait();
|
|
if(servo_channel_mode == SERVO_NEXT_CHANNEL)
|
|
{
|
|
servo_channel_mode = SERVO_SAME_CHANNEL;
|
|
eeprom_write_byte(EEPROM_ADDRESS_SERVO_CHANNEL_MODE,SERVO_SAME_CHANNEL);
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo,channel_pwm.channel_number ); //re-init channel
|
|
}
|
|
else
|
|
{
|
|
servo_channel_mode = SERVO_NEXT_CHANNEL;
|
|
eeprom_write_byte(EEPROM_ADDRESS_SERVO_CHANNEL_MODE,SERVO_NEXT_CHANNEL);
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo,(channel_pwm.channel_number+1)&0x03 ); //re-init channel
|
|
}
|
|
ResetServoChannel(&channel_servo);
|
|
}
|
|
else
|
|
{
|
|
temp_channel = (channel_pwm.channel_number+1) & 0x03; //calculate new channel number; increase, but mask 'a' bit-> always return to usable code
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_pwm, temp_channel ); //re-init channel
|
|
if(servo_channel_mode == SERVO_NEXT_CHANNEL)
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo,(temp_channel+1)&0x03 ); //re-init channel
|
|
else
|
|
OpenPfRx_channel_init( (struct OpenPfRx_channel *)&channel_servo,temp_channel); //re-init channel
|
|
eeprom_busy_wait(); //wait until eeprom available
|
|
eeprom_write_byte(EEPROM_ADDRESS_CHANNEL,temp_channel); //write eeprom byte
|
|
ResetPWMChannel(&channel_pwm);
|
|
ResetServoChannel(&channel_servo);
|
|
}
|
|
sei(); //re-enable interrupts
|
|
}
|
|
}
|
|
}
|
|
|
|
static void ResetPWMChannel(struct OpenPfRx_channel * channel)
|
|
{
|
|
channel->timeout_action = 0;
|
|
channel->A.output_mode = OM_FLOAT;
|
|
channel->B.output_mode = OM_FLOAT;
|
|
channel->A.pwmindex = PWM_FLOAT;
|
|
channel->B.pwmindex = PWM_FLOAT;
|
|
pwmport &= ~(A_C1|A_C2|B_C1|B_C2);
|
|
ocr1_mask_a = 0xFF;
|
|
ocr1_mask_b = 0xFF;
|
|
|
|
}
|
|
|
|
static void ResetServoChannel(struct OpenPfRx_channel * channel)
|
|
{
|
|
channel->timeout_action = 0;
|
|
channel->A.output_mode = OM_FLOAT;
|
|
channel->B.output_mode = OM_FLOAT;
|
|
channel->A.pwmindex = PWM_FLOAT;
|
|
channel->B.pwmindex = PWM_FLOAT;
|
|
servoatrip = SERVO_OFFSET;
|
|
servobtrip = SERVO_OFFSET;
|
|
}
|
|
|
|
static void init_wdt()
|
|
{
|
|
WDTCSR = (1<<WDIE)|(1<<WDCE)|(1<<WDE)|(1<<WDP1)|(1<<WDP0);//0.125s cycle, interrupt, no reset
|
|
}
|
|
|
|
static void go_to_sleep(void)
|
|
{
|
|
cli();
|
|
if (some_condition)
|
|
{
|
|
sleep_enable();
|
|
sei();
|
|
sleep_cpu();
|
|
sleep_disable();
|
|
}
|
|
sei();
|
|
}
|
|
|
|
static void UpdateOutputValues(struct OpenPfRx_output * output)
|
|
{
|
|
switch(output->output_mode)
|
|
{
|
|
case OM_FWD:
|
|
{
|
|
if(output->pwmvalue != OpenPfRx_MIN_PWM_VALUE)
|
|
{
|
|
output->C1 = 1;
|
|
output->C2 = 0;
|
|
}
|
|
else
|
|
{
|
|
//FLOAT
|
|
output->C1 = 0;
|
|
output->C2 = 0;
|
|
}
|
|
break;
|
|
}
|
|
case OM_BWD:
|
|
{
|
|
if(output->pwmvalue != OpenPfRx_MIN_PWM_VALUE)
|
|
{
|
|
output->C1 = 0;
|
|
output->C2 = 1;
|
|
}
|
|
else
|
|
{
|
|
//FLOAT
|
|
output->C1 = 0;
|
|
output->C2 = 0;
|
|
}
|
|
break;
|
|
}
|
|
case OM_FLOAT:
|
|
{
|
|
output->C1 = 0;
|
|
output->C2 = 0;
|
|
break;
|
|
}
|
|
case OM_BRAKE_THEN_FLOAT:
|
|
output->brakethenfloatcount= 0;
|
|
case OM_BRAKE:
|
|
{
|
|
output->C1 = 1;
|
|
output->C2 = 1;
|
|
break;
|
|
}
|
|
case OM_INDEPENDENT:
|
|
default:
|
|
break;
|
|
}
|
|
}
|