/* Servo Recieves: [0][c3][c2][c1][c0][v2][v1][v0] [1][v9][v8][v7][v6][v5][v4][v3] c0-c3 are command v0-v9 are value Commands: 0 Clear ping counter {always obey command} 1 pong if ping counter matches id and inc ping counter {always obey command} 2 listen (servo number) 256 = all {always obey command} 3 ignore (servo number) 256 = all {always obey command} 4 turn servo on {obey if listening} 5 turn servo off {obey if listening} 6 set servo position to argument {obey if listening} 7 get servo position {obey if listening} 15 send device model {obey if listening} Servo Sends: [0][p3][p2][p1][p0][v2][v1][v0] [1][v9][v8][v7][v6][v5][v4][v3] p0-p3 are paramiter number v0-v9 are value Paramiter numbers: 0 pong (value will be servo number) 1 servo position (returns position sensor value) 2 device model (returns 10 bit device model number) */ #include "binary.inc" #include unsigned int dobyte(char data) ; void printReply(unsigned int reply); void printStatus(void) ; unsigned int sendCommand(unsigned int command, unsigned int argument); unsigned char command; unsigned int argument; unsigned char pingCtr; unsigned char listen[4]; unsigned char power[4]; unsigned int cmdpos[4]; unsigned int AdcValues[8]; // servo id numbers #define ID0 0 #define ID1 1 #define ID2 2 #define ID3 3 #define INITPING 0 #define PING 1 #define LISTEN 2 #define IGNORE 3 #define POWERUP 4 #define POWERDOWN 5 #define SETPOSITION 6 #define GETPOSITION 7 #define GETMODEL 15 int main(void) { AdcValues[0] = 25; AdcValues[2] = 50; AdcValues[4] = 75; AdcValues[6] = 100; printf("----------------------------\n"); printf("reset ping counter\n"); printReply( sendCommand(INITPING, 0)); printStatus(); printf("----------------------------\n"); printf("ping\n"); printReply( sendCommand(PING, 0)); printStatus(); printf("----------------------------\n"); printf("ping\n"); printReply( sendCommand(PING, 0)); printStatus(); printf("----------------------------\n"); printf("ping\n"); printReply( sendCommand(PING, 0)); printStatus(); printf("----------------------------\n"); printf("ping\n"); printReply( sendCommand(PING, 0)); printStatus(); printf("----------------------------\n"); printf("ping\n"); printReply( sendCommand(PING, 0)); printStatus(); printf("----------------------------\n"); printf("servo 0 listen\n"); printReply( sendCommand(LISTEN, 0)); printStatus(); printf("----------------------------\n"); printf("power up\n"); printReply( sendCommand(POWERUP, 0)); printStatus(); printf("----------------------------\n"); printf("servo 2 listen\n"); printReply( sendCommand(LISTEN, 2)); printStatus(); printf("----------------------------\n"); printf("set position 64\n"); printReply( sendCommand(SETPOSITION, 64)); printStatus(); printf("----------------------------\n"); printf("all servos ignore\n"); printReply( sendCommand(IGNORE, 256)); printStatus(); printf("----------------------------\n"); printf("servo 1 listen\n"); printReply( sendCommand(LISTEN, 1)); printStatus(); printf("----------------------------\n"); printf("get position\n"); printReply( sendCommand(GETPOSITION, 0)); printStatus(); printf("----------------------------\n"); printf("all servos listen\n"); printReply( sendCommand(LISTEN, 256)); printStatus(); printf("----------------------------\n"); printf("power up\n"); printReply( sendCommand(POWERUP, 0)); printStatus(); printf("----------------------------\n"); printf("power down\n"); printReply( sendCommand(POWERDOWN, 0)); printStatus(); // printReply( (b10000000<<8) | b00010000); return 0; } unsigned int sendCommand(unsigned int command, unsigned int argument){ unsigned char data[2]; data[0] = (command << 3) | (argument & 0x07); data[1] = (argument >> 3 ) | 0x80; printf("%0.2X %0.2X\n", data[0], data[1]); dobyte(data[0]); return dobyte(data[1]); } void printStatus(void) { int n; printf( "Ping counter: %d\n", pingCtr); for (n = 0; n < 4; n++) printf("%d listening: %s\n",n, listen[n]==0?"no":"yes"); for (n = 0; n < 4; n++) printf("%d powered: %s\n",n, power[n]==0?"down":"up"); for (n = 0; n < 4; n++) printf("%d command position: %d\n",n, cmdpos[n]); } void printReply(unsigned int reply) { unsigned char paramiter; unsigned int value; // [0][p3][p2][p1][p0][v2][v1][v0] // [1][v9][v8][v7][v6][v5][v4][v3] paramiter = (reply & 0x78)>>3; value = ((reply & 0x7F00)>>5) | (reply & 0x07); printf(" Paramiter: %d\n", paramiter); printf(" Value: %d\n", value); } unsigned int dobyte(char data) { unsigned int reply; reply = 0; if ((data & 0x80)==0) { //process comamnd on second byte command = (data >> 3); argument = (argument & 0x7F8) | (data & 0x07); // glue in its 0 through 2 } else { argument = (argument & 0x07) | ((data & 0x7F) << 3); //glue in bits 3 through 9 switch(command) { case 0: // Clear ping counter pingCtr = 0; break; case 1: // pong if ping counter matches id and inc ping counter if ((pingCtr == ID0) | (pingCtr == ID1) | (pingCtr == ID2) | (pingCtr == ID3)) { reply = ((pingCtr << 5) & 0xFF00)|(pingCtr & 0x07)|0x8000; } pingCtr++; break; case 2: // listen(id) if (argument == ID0) listen[0] = 1; else if (argument == ID1) listen[1] = 1; else if (argument == ID2) listen[2] = 1; else if (argument == ID3) listen[3] = 1; else if (argument == 256) { listen[0] = 1; listen[1] = 1; listen[2] = 1; listen[3] = 1; } break; case 3: // ignore(id) if (argument == ID0) listen[0] = 0; else if (argument == ID1) listen[1] = 0; else if (argument == ID2) listen[2] = 0; else if (argument == ID3) listen[3] = 0; else if (argument == 256) { listen[0] = 0; listen[1] = 0; listen[2] = 0; listen[3] = 0; } break; case 4: // servo on if (listen[0]) power[0] = 1; if (listen[1]) power[1] = 1; if (listen[2]) power[2] = 1; if (listen[3]) power[3] = 1; break; case 5: // servo off if (listen[0]) power[0] = 0; if (listen[1]) power[1] = 0; if (listen[2]) power[2] = 0; if (listen[3]) power[3] = 0; break; case 6: // set servo position (position) if (listen[0]) cmdpos[0] = argument; if (listen[1]) cmdpos[1] = argument; if (listen[2]) cmdpos[2] = argument; if (listen[3]) cmdpos[3] = argument; break; case 7: // get servo position if (listen[0]) reply = ((AdcValues[0] << 5) & 0xFF00)|(AdcValues[0] & 0x07)|0x8000|(1<<3); else if (listen[1]) reply = ((AdcValues[2] << 5) & 0xFF00)|(AdcValues[2] & 0x07)|0x8000|(1<<3); else if (listen[2]) reply = ((AdcValues[4] << 5) & 0xFF00)|(AdcValues[4] & 0x07)|0x8000|(1<<3); else if (listen[3]) reply = ((AdcValues[6] << 5) & 0xFF00)|(AdcValues[6] & 0x07)|0x8000|(1<<3); break; case 15: // return device model reply = 0x0001; break; } } return reply; }