serial port - Need help fixing C code for pololu maestro -
i trying move multiple servos using c language. using pololu mini maestro 24 channel enter link description here. modified code provided in user manual. pasted compact protocol in manual moving multiple servos. doesn't work. there no errors of warnings code doesn't work. used serial monitor , took peek @ serial output , got this:
[b]aa 0c 1f [u]0d[/u] 0a 07 70 2e 70 2e 70 2e 70 2e 70 2e 70 2e 70 2e 70 2e 70 2e 70 2e [/b]
i don't know why there '0d' in there. not supposed there. oh , getting pot position channel 0 can't move servos. great if can me it.
// uses posix functions send , receive data maestro. // note: maestro's serial mode must set "usb dual port". // note: must change 'const char * device' line below. #include <fcntl.h> #include <stdio.h> #include <unistd.h> #ifdef _win32 #define o_noctty 0 #else #include <termios.h> #endif #include <math.h> int mod (int a, int b) { if(b < 0) //you can check b == 0 separately , want return mod(-a, -b); int ret = % b; if(ret < 0) ret+=b; return ret; } // gets position of maestro channel. // see "serial servo commands" section of user's guide. int maestrogetposition(int fd, unsigned char channel) { unsigned char command[] = {170 ,12 ,16 , channel}; if(write(fd, command, sizeof(command)) == -1) { perror("error writing"); return -1; } unsigned char response[2]; if(read(fd,response,2) != 2) { perror("error reading"); return -1; } return response[0] + 256*response[1]; } // sets target of maestro channel. // see "serial servo commands" section of user's guide. // units of 'target' quarter-microseconds. int maestrosettarget(int fd, unsigned char channel, int target[]) { unsigned char command[] = {170 ,12 ,31 ,10 ,channel , // 0x9f command, 0x0a numbers of channels move, channel first channel number. target[1] & 0x7f, target[1] >> 7 & 0x7f, // channe1 pos, channel 1 speed target[2] & 0x7f, target[2] >> 7 & 0x7f, // ... target[3] & 0x7f, target[3] >> 7 & 0x7f, target[4] & 0x7f, target[4] >> 7 & 0x7f, target[5] & 0x7f, target[5] >> 7 & 0x7f, target[6] & 0x7f, target[6] >> 7 & 0x7f, target[7] & 0x7f, target[7] >> 7 & 0x7f, target[8] & 0x7f, target[8] >> 7 & 0x7f, target[9] & 0x7f, target[9] >> 7 & 0x7f, target[10] & 0x7f, target[10] >> 7 & 0x7f};// ... if (write(fd, command, sizeof(command)) == -1) { perror("error writing"); return -1; } return 0; } int main() { // open maestro's virtual com port. const char * device = "\\\\.\\usbser000"; // windows, "\\\\.\\com6" works //const char * device = "/dev/ttyacm0"; // linux //const char * device = "/dev/cu.usbmodem00034567"; // mac os x int fd = open(device, o_rdwr | o_noctty ); if (fd == -1){ perror(device); return 1; } #ifndef _win32 struct termios options; tcgetattr(fd, &options); options.c_lflag &= ~(echo | echonl | icanon | isig | iexten); options.c_oflag &= ~(onlcr | ocrnl); tcsetattr(fd, tcsanow, &options); #endif int pot = 0; int position = maestrogetposition(fd, pot); printf("current position %d %d.\n", position, pot); int x[10]; x[1] = 6000; x[2] = 6000; x[3] = 6000; x[4] = 6000; x[5] = 6000; x[6] = 6000; x[7] = 6000; x[8] = 6000; x[9] = 6000; x[10] = 6000; maestrosettarget(fd,7, x); close(fd); return 0; }
Comments
Post a Comment