Beginners: please read this post and this post before posting to the forum.
0 Members and 1 Guest are viewing this topic.
//Initial Servo Positions: (forming a Quadped Robot)short int x[] = {0,650,690,660,700,700,700,715,710,235,1130,275,1090,330,1105,270,1090,650,650,640,650,670,575};char lp;#define d(x) delay_ms(x)#define SX(s) switch (s) { //all the 'cases' are actually in the same line !!case 1:servo(PORTJ,6,x[1]);break; case 2:servo(PORTA,6,x[2]);break;case 3:servo(PORTE,7,x[3]);break;case 4:servo(PORTH,2,x[4]);break;case 5:servo(PORTC,1,x[5]);break;case 6:servo(PORTA,5,x[6]);break;case 7:servo(PORTE,2,x[7]);break;case 8:servo(PORTH,6,x[8]);break;case 9:servo(PORTC,0,x[9]);break;case 10:servo(PORTA,4,x[10]);break;case 11:servo(PORTE,3,x[11]);break;case 12:servo(PORTH,5,x[12]);break;case 13:servo(PORTC,3,x[13]);break;case 14:servo(PORTA,2,x[14]);break;case 15:servo(PORTE,4,x[15]);break;case 16:servo(PORTH,4,x[16]);break;case 17:servo(PORTC,4,x[17]);break;case 18:servo(PORTA,1,x[18]);break;case 19:servo(PORTE,5,x[19]);break;case 20:servo(PORTH,3,x[20]);break;case 21:servo(PORTA,7,x[21]);break;case 22:servo(PORTA,3,x[22]);break; }#define SSS for(int i=1;i<24;i++) { SX(i); }//Note: there are 23 loops, but runs 22 servos !for(lp=0;lp<10000;lp++) { d(20); SSS; }
#include "SoR_Utils.h" //includes all the technical stuff/*************************************************Protocol:identification 1st byte - command id = 128servo_on 1st byte - command id = 129 2nd byte - servo number 3rd byte - CRCservo_off 1st byte - command id = 130 2nd byte - servo number 3rd byte - CRCset_servo_position 1st byte - command id = 131 2nd byte - servo number 3rd byte - servo position (bits 1-4) 4th byte - servo position (bits 5-8) 5th byte - CRC for last 4 bytesAll bytes need to be < 128 such that the 5th byte can also be used as a sync byte in case a byteis lost during transmission.If two sync bytes are seen Axon responds with the word Axon in order for the PC to know that it iscommunicating correctly.****************************************************************************/// servo position prior to sortingint init_servo_position[29]={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};// base servo numberint init_servo_number[29]={6,5,4,3,2,7,6,5,4,3,2,1,0,1,2,3,4,5,6,7,6,7,6,5,4,3,2,1,0}; // base servo portint *init_servo_port[29]={&PORTH,&PORTH,&PORTH,&PORTH,&PORTH,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTJ,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC};// sorted servo positionint servo_position[29];// sorted servo numberint servo_number[29];// sorted servo portunsigned int *servo_port[29];// cmd variables are used to maintain state for incoming characters// such that the above protocol can be implemented.int cmd_index = 0;int cmd_id;int cmd_servo_number;int cmd_servo_position_1;int cmd_servo_position_2;int cmd_crc;// defines the timing period of min and max servo values. You may need to adjust these for your servos#define MIN_SERVO_TIME 275#define MAX_SERVO_TIME 1150float fact = ((MAX_SERVO_TIME-MIN_SERVO_TIME)/255.0);// is called when a new character from the PC is availablevoid get_data(unsigned char c){ cli(); //rprintf("%c", c); // toggle LED PORTB ^= (1<<6); // check for our sync byte ... which is also the id if (c&128) { cmd_index=0; cmd_id = (int)c; } // id 128 is use for the PC to know that this is an Axon program if (cmd_id==128) { rprintf("Axon\n"); } else // enable or disble a servo. When a servo is disabled it can be manually moved. if ((cmd_id==129)||(cmd_id==130)) { if (cmd_index==1) { cmd_servo_number=c; } else if (cmd_index==2) { cmd_crc=c; if (((cmd_id^(cmd_servo_number<<1))&127)==cmd_crc) { if (cmd_id==129) init_servo_position[cmd_servo_number]=MIN_SERVO_TIME+(((MAX_SERVO_TIME-MIN_SERVO_TIME)/255)*128); else init_servo_position[cmd_servo_number]=-1; } } } else // used to set a servo position. Note that you need 4 bytes of info for this. if (cmd_id==131) { if (cmd_index==1) cmd_servo_number=c; else if (cmd_index==2) cmd_servo_position_1=c; else if (cmd_index==3) cmd_servo_position_2=c; else if (cmd_index==4) { cmd_crc = (int)c; // CRC is a simple XOR with each successive byte shifted by one more bit if (((cmd_id^(cmd_servo_number<<1)^(cmd_servo_position_1<<2)^(cmd_servo_position_2<<3))&127)==cmd_crc) { // construct the actual value ... we cannot use 1 byte for this since // a servo value ranges from 0-255 which would cause it to be interpreted // as a sync byte for values above 128. Thus we need to use two bytes // to contain half the information. int pos = cmd_servo_position_1|(cmd_servo_position_2<<4); init_servo_position[cmd_servo_number]=MIN_SERVO_TIME+(fact*pos); } } } // increment our data counter cmd_index++;}int main(void){ int i,j; uartInit(); // initialize the UART (serial port) uartSetBaudRate(0, 38400); // set UARTE speed, for Bluetooth uartSetBaudRate(1, 115200); // set UARTD speed, for USB connection uartSetBaudRate(2, 38400); // set UARTH speed uartSetBaudRate(3, 38400); // set UARTJ speed, for Blackfin rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf configure_ports(); timer0Init(); uartSetRxHandler(1, &get_data); //initialize ISR: UCSR1B |= (1 << RXCIE1); // Enable the USART Recieve Complete interrupt (USART_RXCn) sei();//turn on the Global Interrupt Enable flag while (1) { // copy all initial values prior to sorting memcpy(servo_position, init_servo_position, sizeof(servo_position)); memcpy(servo_port, init_servo_port, sizeof(servo_port)); memcpy(servo_number, init_servo_number, sizeof(servo_number)); // sort based on position information and keep track of number and port // we're using a quick and dirty bubble sort which for 29 values should // be ok. You could instead use an insertion sort which might be a little // quicker per loop iteration as we don't expect the position to change // frequently, but with the (N*N)/2 and flag optimizations this should // function fine. int isDone=0; for (i=0;(i<29-1)&&(!isDone);i++) { isDone=1; for (j=0;j<29-i-1;j++) { if (servo_position[j]>servo_position[j+1]) { // swap servo position int tmp = servo_position[j]; servo_position[j] = servo_position[j+1]; servo_position[j+1] = tmp; // keep number and port in sync with the position so we know which port to change! tmp = servo_number[j]; servo_number[j] = servo_number[j+1]; servo_number[j+1] = tmp; tmp = servo_port[j]; servo_port[j] = servo_port[j+1]; servo_port[j+1] = tmp; isDone=0; } } } //turn off interrupts .. this is very important otherwise your servo will jitter! cli(); // set all the ports to on that are enabled (i.e.>0) for (i=0;i<29;i++) { if (servo_position[i]>0) { PORT_ON(*servo_port[i], servo_number[i]); } } // delay until the last position has occured int offset=0; for (i=0;i<29;i++) { if (servo_position[i]>0) { if (servo_position[i]>offset) { // wait until this servo's position offset delay_cycles(servo_position[i]-offset); offset=servo_position[i]; } // turn off this servo and continue with next servos PORT_OFF(*servo_port[i], servo_number[i]); } } // renable interrupts so that we can receive UART/serial values sei(); // wait for a total of 20ms from the last servo timing value. timerPause((4666-offset)/234); }}
How does RoboRealm software keeps all the servos tight without vibration !!?
// set all the ports to on that are enabled (i.e.>0) for (i=0;i<29;i++) { if (servo_position[i]>0) { PORT_ON(*servo_port[i], servo_number[i]); } } // delay until the last position has occured int offset=0; for (i=0;i<29;i++) { if (servo_position[i]>0) { if (servo_position[i]>offset) { // wait until this servo's position offset delay_cycles(servo_position[i]-offset); offset=servo_position[i]; } // turn off this servo and continue with next servos PORT_OFF(*servo_port[i], servo_number[i]);