use arduino uno code, below are sample

#define SET_DDRB 0x2D // b 0010 1101 #define SET_PORTB 0x00

// PORTC.0 0x01 A0 23 I ADC0 CDS Light sensor // PORTC.1 0x02 A1 24 O ADC1 S1 // PORTC.2 0x04 A2 25 O ADC2 S2 // PORTC.3 0x08 A3 26 O ADC3 X1 // PORTC.4 0x10 A4 27 O ADC4 X2 // PORTC.5 0x20 A5 28 O ADC5 X3 // PORTC.6 0x40 29 x RESET

#define SET_DDRC 0xFE // b 1111 1110 #define SET_PORTC 0x00

// PORTD.0 0x01 0 30 I Rx RX SERIAL Data // PORTD.1 0x02 1 31 O Tx TX SERIAL Data // PORTD.2 0x04 2 32 O INT0 NC // PORTD.3 0x08 3 1 O OC2B/INT1 NC // PORTD.4 0x10 4 2 O XCK/T0 SERVO 0 // PORTD.5 0x20 5 9 O OC0B/T1 SERVO 1 // PORTD.6 0x40 6 10 O OC0A/AIN0 SERVO 2 // PORTD.7 0x80 7 11 O AIN1 SERVO 3

#define SET_DDRD 0xFE // b 1111 1110 #define SET_PORTD 0x00

// ADC6 19 x ADC6 NC // ADC7 22 x ADC7 NC

volatile uint8_t tick; // ++ every 4mS uint8_t tickX;

uint8_t time4mS; uint8_t time100mS; uint32_t time1S;

#define TX_BUF_SIZE 64 #define TX_BUF_MASK 0x3F

#define RX_BUF_SIZE 64 #define RX_BUF_MASK 0x3F

volatile char txBuf[TX_BUF_SIZE]; volatile uint8_t txBufInPtr; volatile uint8_t txBufOutPtr;

volatile char rxBuf[RX_BUF_SIZE]; volatile uint8_t rxBufInPtr; volatile uint8_t rxBufOutPtr;

char cmdBuf[16]; uint8_t cmdBufPtr;

volatile uint16_t servo[4];

volatile uint8_t servoState;

#define S_HIGH 35000 #define S_LOW 45000 #define CENTER 40000

uint8_t stateA; uint8_t stateB; uint8_t stateC; // run servos

uint8_t sspeed;

SIGNAL(USART_UDRE_vect){ if(txBufOutPtr == txBufInPtr){ // no more char to output UCSR0B &= ~0x20; } else { // we have more UDR0 = txBuf[txBufOutPtr++]; txBufOutPtr &= TX_BUF_MASK; } }

SIGNAL(USART_RX_vect){ rxBuf[rxBufInPtr++] = UDR0; rxBufInPtr &= RX_BUF_MASK; }

SIGNAL(TIMER1_OVF_vect){ TCCR1B = 0x00; // kill timer1 switch(servoState){ case 0: PORTD &= ~0x10; // kill servo 0 PORTD |= 0x20; // start servo 1

TCCR1A = 0x00; TCNT1 = servo[1]; TIMSK1 = 0x01; // interrupts for timer 1 on TCCR1B = 0x01; // start counting servoState = 1;

break; case 1: PORTD &= ~0x20; // kill servo 1 PORTD |= 0x40; // start servo 2

TCCR1A = 0x00; TCNT1 = servo[2]; TIMSK1 = 0x01; // interrupts for timer 1 on TCCR1B = 0x01; // start counting */ servoState = 2;

break; case 2: PORTD &= ~0x40; // kill servo 1 break; } }

void inline processServos(){ // this is called every 20mS PORTD |= 0x10; servoState = 0; TCCR1A = 0x00; TCNT1 = servo[0]; TIMSK1 = 0x01; // interrupts for timer 1 on TCCR1B = 0x01; // start counting }

SIGNAL(TIMER0_COMPA_vect){ static uint8_t i = 0; PINC = 0x08; tick++; i++;

if(i > 4){ PINC = 0x10; i = 0; processServos(); }

}

void busyWait(){ volatile uint32_t i; for(i=0;i

void outCharOld(char c){ while((UCSR0A & 0x20) == 0); // we don't want to do this UDR0 = c; }

void outChar(char c){ txBuf[txBufInPtr++] = c; txBufInPtr &= TX_BUF_MASK; UCSR0B |= 0x20; }

void printHex4(uint8_t n){ n = n & 0x0F; n = n + '0'; if(n > '9'){ n += 7; }

outChar(n); }

void printHex8(uint8_t n){ printHex4(n >> 4); printHex4(n); }

void printHex16(uint16_t n){ printHex8(n >> 8); printHex8(n); }

void printHex32(uint32_t n){ printHex16(n >> 16); printHex16(n); }

void printStatus(){ printHex8(rxBufInPtr); outChar(' '); printHex32(time1S); outChar(0x0A); }

void printString(char *s){ while(s[0]){ outChar(s[0]); s++; } }

uint16_t getArg1(){ // decimal uint16_t n; uint8_t i; i = 1; n = 0;

while(cmdBuf[i] == ' '){ i++; } while(cmdBuf[i] >= '0' && cmdBuf[i]

void processCommand(){ outChar('('); printString(cmdBuf); outChar(')'); outChar(0x0A);

switch(cmdBuf[0]){ case '0': servo[0] = getArg1(); break; case '1': servo[1] = getArg1(); break; case 'a': printString("hit an a"); servo[1] = S_HIGH; break; case 'b': printString("some stuff here");

break; case 'c': printString("other stuff"); break; case 'd': printString("hello"); break; case 'A': stateA = getArg1(); break; case 'B': stateB = getArg1(); break;

case 'C': stateC = getArg1(); break;

case 's': sspeed = getArg1(); break; } }

void processChar(){ char c; c = rxBuf[rxBufOutPtr++]; rxBufOutPtr &= RX_BUF_MASK; if(cmdBufPtr

void led1On(){ PORTB |= 0x20; }

void led1Off(){ PORTB &= ~0x20; }

void led2On(){ PORTB |= 0x04; }

void led2Off(){ PORTB &= ~0x04; }

void stateMachineA(){ // called 10 times every second switch(stateA){ case 0: led1On(); stateA=1; break; case 1: led1Off(); stateA=2; break; case 2: stateA=0; break; } }

void stateMachineB(){ // called 1 times every second switch(stateB){ case 0: led2On(); stateB=1; break; case 1: led2Off(); stateB=2; break; case 2: stateB=0; break; } }

void stateMachineC(){ // called 10 times every second static uint8_t timer; if(timer){ timer--; } switch(stateC){ case 0: servo[0] = S_HIGH; if(timer == 0){ timer = sspeed; stateC=1; } break; case 1: servo[1] = S_HIGH; if(timer == 0){ timer = sspeed; stateC=2; } break; case 2: servo[0] = S_LOW; if(timer == 0){ timer = sspeed; stateC=3; } break; case 3: if(timer == 0){ stateC=0; timer = sspeed; } servo[1] = S_LOW; break; } }

void process100mS(){ stateMachineA(); stateMachineC();

}

void process1S(){ static uint8_t i=0; stateMachineB(); printStatus(); i++; // if(i & 0x01){ // servo[0] = S_HIGH; // } else { // servo[0] = S_LOW; // } }

int main(){ uint16_t i; DDRB = SET_DDRB; DDRC = SET_DDRC; DDRD = SET_DDRD; PORTB = SET_PORTB; PORTC = SET_PORTC; PORTD = SET_PORTD;

// tick timer setup TCCR0A = 0x02; TCCR0B = 0x04; // divide by 8 OCR0A = 250; TIMSK0 = 0x02; SREG |= 0x80; // setup uart UBRR0H = 0; UBRR0L = 103; UCSR0B = 0x98; // 1001 1000

tick = 0;

i = 0;

time1S = 0;

stateA = 0; stateB = 0; stateC = 0; sspeed = 10; servo[0] = CENTER; servo[1] = CENTER; servo[2] = CENTER; servo[3] = CENTER;

while(1){ PINC = 0x20;

if(tick != tickX){ time4mS++; tickX++; if(time4mS > 24){ time100mS++; time4mS = 0; process100mS(); if(time100mS > 9){ time1S++; time100mS = 0; process1S(); } } }

while(rxBufInPtr != rxBufOutPtr){ processChar(); } } // end of while(1) }

Your job is to create a function that you call like this void setServo(uint8 t n,uint16_t p) // n is the servo we wish to set /p is the angle in degrees we want. // Each of the 4 servos will have different calibration values Your code goes here