Code:
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#ifdef _WIN32
#define O_NOCTTY 0
#else
#include <termios.h>
#endif
// Gets the position of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
int maestroGetPosition(int fd, unsigned char channel)
{
unsigned char command[] = {0x90, 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 the target of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
// The units of 'target' are quarter-microseconds.
int maestroSetTarget(int fd, unsigned char channel, unsigned short target)
{
unsigned char command[] = {0x84, channel, target & 0x7F, target >> 7 & 0x7F};
if (write(fd, command, sizeof(command)) == -1)
{
perror("error writing");
return -1;
}
return 0;
}
int main()
{
// Open the Maestro's virtual COM port.
const char * device = "\\\\.\\USBSER000"; // Windows, "\\\\.\\COM6" also 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
#define m 6
int matrix[m][m] ={0};
{
FILE *fp;
int i,j,k,l,b,v;
int position= maestroGetPosition(fd, 0);
int target;
fp= fopen("Werte", "r");
if (fp==NULL)
{
printf("Can`t open file");
exit(-1) ;
}
for(i=0; i<m;i++)
{
fscanf(fp,"%d",& matrix[0][i]);
fflush( stdout );
sleep(3);
target=matrix[0][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 0, target);
}
for(j=0; j<m;j++)
{
fscanf(fp,"%d",& matrix[1][j]);
fflush( stdout );
sleep(2);
target=matrix[1][j];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 1, target);
}
for(k=0; k<m;k++)
{
fscanf(fp,"%d",& matrix[2][k]);
fflush( stdout );
sleep(2);
target=matrix[2][k];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 2, target);
}
for(l=0; l<m;l++)
{
fscanf(fp,"%d",& matrix[3][l]);
fflush( stdout );
sleep(2);
target=matrix[3][l];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 3, target);
}
for(b=0; b<m;b++)
{
fscanf(fp,"%d",& matrix[4][b]);
fflush( stdout );
sleep(2);
target=matrix[4][b];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 4, target);
}
for(v=0; v<m;v++)
{
fscanf(fp,"%d",& matrix[5][v]);
fflush( stdout );
sleep(2);
target=matrix[5][v];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 5, target);
}
close(fd);
}
return 0;
}
Ich weiß, ist nicht gerade die beste Lösung aber meine Kenntnisse in C sind nicht die besten.
Zitieren
Zitat von Geistesblitz
Lesezeichen