![icy [labs]](/moin_static171/common/wikilogo.png)
Servo Code
Makefile
NAME = servo
DEVICE = atmega32
CLOCK = 16000000
LIB_PATH = ../../penguino-lib/
LIB_OBJS = $(LIB_PATH)penguino/uart/RingBuff.o \
$(LIB_PATH)penguino/uart/uart.o \
$(LIB_PATH)penguino/uart/uart-stdio.o \
$(LIB_PATH)penguino/spi/spi-master.o \
$(LIB_PATH)penguino/io.o
OBJECTS = $(NAME).o $(LIB_OBJS)
CFLAGS = -I$(LIB_PATH)
COMPILE = avr-gcc -Wall -Os $(CFLAGS) -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -std=gnu99
# symbolic targets:
all: $(NAME).bin
upload: $(NAME).bin
flip $(NAME).bin
.c.o:
$(COMPILE) -c $< -o $@
.S.o:
$(COMPILE) -x assembler-with-cpp -c $< -o $@
.c.s:
$(COMPILE) -S $< -o $@
clean:
rm -f $(NAME).bin $(NAME).elf $(OBJECTS)
$(NAME).elf: $(OBJECTS)
$(COMPILE) -o $(NAME).elf $(OBJECTS)
$(NAME).bin: $(NAME).elf
rm -f $(NAME).bin
avr-objcopy -j .text -j .data -O binary $(NAME).elf $(NAME).bin
disasm: $(NAME).elf
avr-objdump -d $(NAME).elf
1
2 /*
3 * servo.c
4 */
5
6 #include <stdio.h>
7 #include <string.h>
8
9 #include "../../penguino-lib/penguino/uart/uart.h"
10 #include "../../penguino-lib/penguino/uart/uart-stdio.h"
11 #include "../../penguino-lib/penguino/io.h"
12 #include "../../penguino-lib/penguino/time.h"
13
14 #define UART_BAUD_RATE 115200
15 #define NUM_SERVOS 4
16
17 typedef struct {
18 uint16_t minAngleDelay;
19 uint16_t maxAngleDelay;
20 uint16_t maxAngle;
21 Pin pin;
22 uint16_t homeAngle;
23 } servo_t;
24
25 static inline void servo_init(
26 servo_t *servo,
27 uint16_t minAngleDelay,
28 uint16_t maxAngleDelay,
29 uint16_t maxAngle,
30 Pin pin,
31 uint16_t homeAngle
32 ) {
33
34 servo->minAngleDelay = minAngleDelay;
35 servo->maxAngleDelay = maxAngleDelay;
36 servo->maxAngle = maxAngle;
37 servo->pin = pin;
38 servo->homeAngle = homeAngle;
39
40 output_init( pin );
41 }
42
43 static inline void servo_turnToAngle( servo_t *servo, uint16_t angle ) {
44 uint32_t delayRange = servo->maxAngleDelay-servo->minAngleDelay;
45 uint16_t delayOffset = ((uint32_t)angle*delayRange)/servo->maxAngle;
46 uint16_t delayTime = servo->minAngleDelay + delayOffset;
47 drivePin( servo->pin, High );
48 _delay_us( delayTime );
49 drivePin( servo->pin, Low );
50 }
51
52
53 int main( void ) {
54 statusLed_init( );
55
56 // flash status LED for a bit
57 statusLed_orange( );
58 delay_ms( 500 );
59 statusLed_off( );
60
61 // initalise UART
62 uart_init( UART_BAUD_RATE );
63
64 // enable UART as stdio
65 uart_stdio_init( );
66
67 // enable echo on the UART stdin so the user can see what they are typing
68 uart_stdio_echo( true );
69
70
71 servo_t servos[NUM_SERVOS];
72
73 Pin pins[NUM_SERVOS] = {0};
74 uint16_t angles[NUM_SERVOS] = {0};
75 pins[0] = B0;
76 pins[1] = B1;
77 pins[2] = B2;
78 pins[3] = B3;
79
80 uint8_t i;
81
82 for ( i = 0; i != NUM_SERVOS; ++i ) {
83 servo_init( &servos[i], 60, 2000, 170, pins[i], 170/2 );
84 }
85
86 printf("START\r\n");
87
88 for ( i = 0; i != NUM_SERVOS; ++i ) {
89 angles[i] = servos[i].homeAngle;
90 servo_turnToAngle( &servos[i], angles[i] );
91 }
92
93 delay_ms( 10 );
94
95 uint8_t direction = 0;
96 uint8_t speed = 1;
97
98 while ( 1 ) {
99
100 for ( i = 0; i != NUM_SERVOS; ++i ) {
101 servo_turnToAngle( &servos[i], angles[i] );
102
103 if ( angles[i] == 0 ) {
104 angles[i] = 1;
105 direction = 1 - direction;
106 } else if ( angles[i] >= servos[i].maxAngle ) {
107 angles[i] = servos[i].maxAngle-1;
108 direction = 1 - direction;
109 }
110
111 if ( direction ) {
112 angles[i] += speed;
113 } else {
114 angles[i] -= speed;
115 }
116 }
117
118 delay_ms( 10 );
119
120 }
121 return 0;
122 }
123
to compile and upload:
make upload