[code]//
/* /
/ File: hexapod.c /
/ /
/ Version: 1.00 /
/ /
/ Interface of: 18 servo hexapod /
/ /
/ Date: 5-1-2009 /
/ /
/ Authors: Martijn Jonkers /
/ /
/ Change history: /
/ date: who: changes: /
/ ---- — ------- /
/ */
//
#include <pgmspace.h>
#include <io.h>
#include <stdlib.h>
#include <interrupt.h>
#include <string.h>
#include <hexapod.h>
//
/* /
/ Movements /
/ */
//
#define NumForwardSteps 5
int Forward[NumForwardSteps][NumLegs][NumServosLeg] =
{ //leg1 LEG2 LEG3 LEG4 LEG5 LEG6
{{ICNTR,UP ,FAR} ,{STAY ,STAY,STAY} ,{RIGHT,UP ,STAY} ,{STAY,STAY,STAY} ,{LEFT ,UP ,STAY} ,{STAY ,STAY,STAY }},//11
{{STAY ,DOWN,STAY} ,{STAY ,STAY,STAY} ,{STAY ,DOWN,CLOSE},{STAY,STAY,STAY} ,{STAY ,DOWN,CLOSE},{STAY ,STAY,STAY }},//12
{{STAY ,STAY,CLOSE},{RIGHT,UP ,STAY} ,{LEFT ,STAY,OCNTR},{STAY,UP ,CLOSE},{RIGHT,STAY,OCNTR},{LEFT ,UP ,STAY }},//7
{{STAY ,STAY,STAY} ,{STAY ,DOWN,OCNTR},{STAY ,STAY,STAY} ,{STAY,DOWN,STAY} ,{STAY ,STAY,STAY} ,{STAY ,DOWN,OCNTR}},//13
{{STAY ,UP ,FAR } ,{LEFT ,STAY,CLOSE},{RIGHT,UP ,STAY} ,{STAY,STAY,FAR} ,{LEFT ,UP ,STAY} ,{RIGHT,STAY,CLOSE}} //7
};
#define NumInitSteps 2
int InitLegs[NumInitSteps][NumLegs][NumServosLeg] =
{ //leg1 LEG2 LEG3 LEG4 LEG5 LEG6
{{ICNTR,DOWN,OCNTR},{STAY ,STAY,STAY} ,{ICNTR,DOWN,OCNTR},{STAY ,STAY,STAY} ,{ICNTR,DOWN,OCNTR},{STAY ,STAY,STAY }},//9
{{STAY ,STAY,STAY} ,{ICNTR,DOWN,OCNTR},{STAY ,STAY,STAY} ,{ICNTR,DOWN,OCNTR},{STAY ,STAY,STAY} ,{ICNTR,DOWN,OCNTR}} //9
};
#define NumRotateRightSteps 4
int TurnRight[NumRotateRightSteps][NumLegs][NumServosLeg] =
{ //leg1 LEG2 LEG3 LEG4 LEG5 LEG6
{{RIGHT,UP ,STAY},{LEFT ,STAY,STAY},{RIGHT,UP ,STAY},{LEFT ,STAY,STAY},{RIGHT,UP ,STAY},{LEFT ,STAY,STAY}},//
{{STAY ,DOWN,STAY},{STAY ,STAY,STAY},{STAY ,DOWN,STAY},{STAY ,STAY,STAY},{STAY ,DOWN,STAY},{STAY ,STAY,STAY}},//
{{LEFT ,STAY,STAY},{RIGHT,UP ,STAY},{LEFT ,STAY,STAY},{RIGHT,UP ,STAY},{LEFT ,STAY,STAY},{RIGHT,UP ,STAY}},//
{{STAY ,STAY,STAY},{STAY ,DOWN,STAY},{STAY ,STAY,STAY},{STAY ,DOWN,STAY},{STAY ,STAY,STAY},{STAY ,DOWN,STAY}} //
};
//
/* /
/ INITIALISATIE FUNCTIONS /
/ */
//
void init_devices (void)
{
cli (); /* Disable global interrupts /
port_init (); / Roep de functie om de poorten te initializeren aan /
timer1_init (); / Roep de functie om de timer te initializeren aan /
uart_init (); / Roep de functie om de uart te initializeren aan */
MCUCR = 0x00; /* Zoek betekenis hiervan op in de datasheet*/
GIMSK = 0x00; /* Zoek betekenis hiervan op in de datasheet*/
sei (); /* Enable global interrupt */
}
void port_init (void)
{
PORTB = 0xFF; /* Pull-ups enabled /
DDRB = 0x83; / PortB pins 0,1,7 are outputs, rest inputs */
}
void timer1_init (void)
{
TCCR1A = 0x00;
TCCR1B = 0x00; /* Stop timer during init /
OCR1A = 0x059D; / Compare value = 1437 (10Hz) /
TIMSK = 0x10; / CompareA interrupt enable /
TCCR1B = 0x0B; / Clear on compare, presc. = 64 */
}
void uart_init (void)
{
UCSRB = 0x1C; /* Transmitter/Receiver enabled */
UBRRL = 0x05;
}
//
/* /
/ ISR /
/ */
//
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
// PORTB = PORTB ^ 0x02; /* Toggle PB1 value (^ = EXOR) /
/ Hoog de teller op */
}
STATE state(void)//check current status
{
Uart_SendString(“Q\r”);// + \n for terminal use
switch(Uart_getchar())
{
case ‘.’:
status = READY;
break;
case ‘+’:
status = BUSY;
break;
default:
break;
}
return status;
}
void wait(void)//wai the movement to complete
{
while(state()==BUSY){}
}
//
/* /
/ FUNCTIES /
/ */
//
void Uart_SendString (char p)
{
unsigned int uiCharpointer = 0; / Wijzer naar eerste karakter in de string */
while (p[uiCharpointer] != '\0') /* Einde van de string? */
{
while ( !(UCSRA & (1<<UDRE)) );
UDR = p[uiCharpointer]; /* Plaats huidige karakter in verzendregister */
uiCharpointer++; /* Wijzer naar het volgende karakter */
}
}
void Uart_Send_P(const char *addr)
{
char c;
while ((c = pgm_read_byte(addr++)))
Uart_putchar©;
}
void Uart_putchar(char c)
{
loop_until_bit_is_set(UCSRA, UDRE);
UDR = c;
}
char Uart_getchar(void)
{
char input = ‘\0’;
while (input == ‘\0’) //Wait for answer from JB
while (UCSRA & (1<<RXC)) //Do when RXC is true
input = UDR; //Receive one Character and put it in: input
return input;
}
//
/ /
/ Move /
/ /
//
int move(int direction][NumLegs][NumServosLeg],int numSteps)
{
char string[400]="";
char temp[5]="";
int status=1;
int legCounter=0;
int servoCounter=0;
int step=0;
int chnl=0;
for(step=0;step<numSteps;step++)
{
for(legCounter=0;legCounter<NumLegs;legCounter++)
{
for(servoCounter=0;servoCounter<NumServosLeg;servoCounter++)
{
if(direction[step][legCounter][servoCounter]!=STAY)
{
strcat(string,"#");
itoa(channel[chnl],temp,10);
strcat(string,temp);
strcat(string,"P");
itoa((direction[step][legCounter][servoCounter]+offset[channel[chnl]]),temp,10);
strcat(string,temp);
}
chnl++;
}
}
strcat(string,"T");
strcat(string,SPEED);
strcat(string,"\r");
Uart_SendString(string);
//Uart_SendString("\r\n");//test
strcpy(string,"");
wait();
chnl=0;
}
if(error)
status = 0;
return status;
}
//
/ /
/ All servo’s off /
/ /
//
void all_off(void)
{
char temp[3];
int motor = 0;
for(motor=0;motor<=31;motor++)
{
itoa(motor,temp,10);//convert channel number
Uart_SendString("#");
Uart_SendString(temp);
Uart_SendString(" L \r");// \n for terminal use
}
wait();
}
//
/* /
/ MAIN /
/ */
//
int main (void)
{
init_devices(); /* Roep de initialisatie functies aan */
//Uart_Send_P(PSTR("#0 P1600 #1P1500 #12 P1600 #13 P1500 #27 P1500 #26 P1500 T2500 \r"));
move(InitLegs,NumInitSteps) ; //init robot
int i=0;
do
{
move(Forward,NumForwardSteps);
i++;
} while (i<10);
all_off();
while(1)
{};
return 0; /* Eindeloze lus */
}
//
/* /
/ EINDE VAN HET PROGRAMMA /
/ */
//
[/code]
[code]//
/* /
/ File: hexapod.h /
/ /
/ Version: 1.00 /
/ /
/ Interface of: hexapod /
/ /
/ Date: 5-1-2009 /
/ /
/ Authors: Martijn Jonkers /
/ /
/ Change history: /
/ date: who: changes: /
/ ---- — ------- /
/ */
//
//
/* /
/ Defines /
/ */
//
#define NumLegs 6 //number of legs
#define NumServosLeg 3 //number of servo’s per leg
#define NumServos (NumLegs*NumServosLeg) //total number of servo’s
#define LEFT 1750 //inner servo left
#define RIGHT 600 //inner servo right
#define ICNTR 1150 //inner servo center
#define UP 1500 //middle servo up
#define DOWN 1150 //middle servo down
#define MCNTR 1350 //middle servo middle
#define FAR 1000 //outer servo far
#define CLOSE 1600 //outer servo close
#define OCNTR 1400 //outer servo middle
#define STAY 0 //0 Lets servo stay at current position
#define SPEED “2500” //robot movement speed (ms per move)
typedef enum {BUSY, READY} STATE;//robot state (moving/ready)
typedef enum {OK, WRONG} ERROR; //error state
#define UDRE 5
#define BAUD 9600 //baud rate
//
/* /
/ Functions /
/ */
//
void init_devices (void);
void port_init (void);
void timer1_init (void);
void uart_init (void);
void Uart_putchar(char c);
char Uart_getchar(void);
void Uart_SendString (char *p);
STATE state(void);
void wait(void);
void all_off(void);
int move(int direction][NumLegs][NumServosLeg],int numSteps);
//
/* /
/ Global variables /
/ */
//
STATE status = BUSY;
ERROR error = OK;
int channel[NumServos] = //all servo channels
{//inner middle outer
2, 1, 0, //leg 1
6, 5, 4, //leg 2
14, 13, 12, //leg 3
29, 30, 31, //leg 4
25, 26, 27, //leg 5
17, 18, 19 //leg 6
};
int offset[32] = //offset for all 32 channels
{
0, //0
0, //1
0, //2
0, //3
0, //4
0, //5
0, //6
0, //7
0, //8
0, //9
0, //10
0, //11
0, //12
0, //13
0, //14
0, //15
0, //16
0, //17
0, //18
0, //19
0, //20
0, //21
0, //22
0, //23
0, //24
0, //25
-100, //26
100, //27
0, //28
0, //29
0, //30
0 //31
};
//
/* /
/ End /
/ */
//
[/code]
This is the complete code i use…
I hope someone can help me…
It is just such a small thing… and i just can’t get it work :S