/* 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<