/*
Firmware for "Raspberry Pi Servo board" Version 0.2 2013-01-20
Copyright 2012 Mikael Johansson
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
*/
#define F_CPU 20000000UL // CPU Freq for delay.h below
#include
#include
#include
#include
#include
#include
#define USART_BPS_9600 0
#define USART_BPS_19200 1
#define USART_BPS_38400 2
#define USART_BPS_57600 3
#define USART_BPS_115200 4
#define USART_BPS_230400 5
#define USART_BPS_500000 6
// Should be enough for longest possible cmd or parameter, including one space:
#define INPUT_BUFFER_SIZE 8
void setup_usart(unsigned short speed);
unsigned short promille_to_timer(signed short promille);
void Process_handler(void);
void RX_handler(void);
void TX_handler(void);
void MSG_handler(void);
enum {
IDLE,
REC_CMD,
REC_DATA
} state;
// Used to buffer input from serial port one cmd/parameter at a time.
unsigned char buf[INPUT_BUFFER_SIZE];
unsigned char bufpos = 0;
// Used to save the cmd as ascii, while parsing parameters.
unsigned char cmd[INPUT_BUFFER_SIZE];
// Used to save the parameters converted to a number while parsing.
signed short param[8];
// Used for sending both NACK and ACK to save some precious RAM.
const unsigned char const *acknack = "NACK\r\n";
#define ACK_STR (acknack+1)
#define NACK_STR (acknack)
#define VERSION_STR "02\r\n"
union {
unsigned char ucAll;
unsigned char ucAny;
struct {
unsigned char exec_CMD : 1;
unsigned char send_ACK : 1;
unsigned char send_NACK : 1;
unsigned char send_VER : 1;
unsigned char run_test : 1;
unsigned char change_bps : 1;
unsigned char ignore_ws : 1;
unsigned char TX : 1;
unsigned char enable : 1;
};
} uFlags;
// This array contains the target servo position.
unsigned short servo_target[8] = {30000, 30000, 30000, 30000, 30000, 30000, 30000, 30000};
//This array is read by Interrupt and contains current servo position.
volatile unsigned short servo[8] = {30000, 30000, 30000, 30000, 30000, 30000, 30000, 30000};
//This array contains the how much the servo should move every ms.
unsigned char servo_step[8] = {0};
//The index of wanted uart speed, ACK is sent before speed is changed.
unsigned char bps_index = 0;
// Clear pulse to all servos
SIGNAL(SIG_TIMER1_COMPA)
{
PORTB = 0x00;
}
// Get pulse length for current servo and set up timer1 for it and set correct pin high.
SIGNAL(SIG_TIMER0_COMPA)
{
static volatile unsigned char current_servo = 0; // Just used here, volatile not needed?
// not needed: TCNT0 = 0x00;
TIFR |= (1 << OCF1A);
current_servo = (current_servo + 1) & 0x07;
OCR1A = servo[current_servo] - 18; // 18 is for ISR setup time, by measuring output.
if(uFlags.enable)
{
PORTB |= (1 << current_servo);
}
TCNT1 = 0x0000;
}
int main(void)
{
uFlags.ucAll = 0; // Clear all flags
// 0xFFFF can never be a real value, assume eeprom is not initialized.
if(eeprom_read_word((void *)0) != 0xFFFF)
{
unsigned char i;
for(i=0;i<8;i++)
{
servo[i] = eeprom_read_word((void *)(i<<1));
}
}
//PORTS
DDRB = 0xFF; // All servo pins are outputs.
DDRD = 0x02; // Only serial TX pin output on portD.
//TIMERS
// 8-bit timer - interval between the eight servo pulses.
TCCR0A = _BV(WGM01); // CTC
TCCR0B = _BV(CS02); // clk/256
// 16-bit timer - length of pulse
TCCR1B = _BV(CS10) | _BV(WGM12); // CTC and clk/1.
TIMSK |= (1 << OCIE1A) | (1 << OCIE0A); //Enable interrupt for CompareA-match for timer 0 and 1.
OCR0A = 195; // Makes pulse starting interrupt run ~8 times per 20ms (Timer 0)
// UART
UCSRA |= (1<1900)
promille = 1900;
// Less code than promille*10+30000 ?
return (promille+promille+promille+promille+promille+promille+promille+promille+promille+promille+30000);
}
void Process_handler(void)
{
unsigned char i;
unsigned char tickflag = 0;
static unsigned char twentys_of_ms;
twentys_of_ms++;
if(twentys_of_ms == 20)
{
twentys_of_ms = 0;
tickflag = 1;
}
if(uFlags.run_test)
{
unsigned char i;
static unsigned char dir = 1;
static unsigned short tmp=30000;
for(i=0;i<8;i++)
servo[i] = tmp;
if(tmp == 40000)
dir = 0;
if(tmp == 20000)
dir = 1;
if(dir == 1)
tmp = tmp+1;
else
tmp = tmp-1;
}
// Promille / ms
if(tickflag)
{
tickflag = 0;
for(i=0;i<8;i++)
{
if(servo[i] != servo_target[i]) // Not yet at target?
{
//No
if(servo_step[i] != 0) // Supposed to update?
{
//Yes
if(abs((servo_target[i] - (signed long)servo[i])) <= (signed long)servo_step[i]) // One step is bigger than remaining distance to target?
servo[i] = servo_target[i]; // Yes - just set servo´to target, done!
if(servo[i] != servo_target[i]) // Are we there yet?
{
//No - Move servo in correct direction
if((servo_target[i] - (signed long)servo[i]) > 0)
servo[i] += (servo_step[i]);
else
servo[i] -= (servo_step[i]);
}
}
}
}
}
}
void RX_handler(void)
{
static unsigned char n = 0;
unsigned char rx;
unsigned char i;
// Have we received a new byte?
if ( (UCSRA & (1<