test/angleTest.cpp

Go to the documentation of this file.

#include <iostream>
#include <termios.h>
#include <errno.h>
#include <fcntl.h>
#include "../truckd/servo.h"

using std::cin;
using std::cout;
using std::cerr;
using std::endl;




int main(int argc, char * argv[])
{
        int servo_fd;
  int result;
        
        int servoNum;

        char *servo_dev = "/dev/ttyS1"; 


        // grab servo number
        cout << "Enter servo number (0 to 7) > ";
        cin >> servoNum;
        if (!cin) {
                cerr << "Invalid input" << endl;
                return 1;
        }
        if ((servoNum < 0) || (servoNum > 7)) {
                cerr << "Servo number not between 0 and 7" << endl;
                return 1;
        }


        float minAngle, minValue ;
        float maxAngle, maxValue;

        // get angle servo mapping
        cout << "Enter minimum angle and servo value for it > ";
        cin >> minAngle >> minValue;
        if (!cin) {
                cerr << "Invalid input" << endl;
                return 1;
        }

        // get angle servo mapping
        cout << "Enter maximum angle and servo value for it > ";
        cin >> maxAngle >> maxValue;
        if (!cin) {
                cerr << "Invalid input" << endl;
                return 1;
        }

        if (minAngle >= maxAngle) {
                cerr << "MIN angle must be less than MAX angle" << endl;
                return 1;
        }

        cout << "MIN angle : " << minAngle << "  value : " << minValue << endl
                         << "MAX angle : " << maxAngle << "  value : " << maxValue << endl;

        Servo s(servoNum, 0, minAngle, minValue, maxAngle, maxValue);


  // open serial port for controlling servos / speed controllers
        struct termios newtio;
        servo_fd = open(servo_dev, O_WRONLY | O_NOCTTY | O_NONBLOCK);
        
        if (servo_fd < 0)
        {       perror("hwInterface : opening serial port failed");
                exit(EXIT_FAILURE);
        }
        // set new port settings for 9600 baud, ignore control lines
        newtio.c_cflag = CS8 | B9600 | CLOCAL;
        newtio.c_iflag = IGNPAR;
        newtio.c_oflag = 0;
        newtio.c_lflag = 0;
        newtio.c_cc[VMIN]=1;
        newtio.c_cc[VTIME]=0;
        tcflush(servo_fd, TCIFLUSH);
        result = tcsetattr(servo_fd,TCSANOW,&newtio);
        
        if (result == -1) {
                perror("hwInterface tcsetattr failed");
                return 1;
        }

        float angle;

        do {
                cout << "Enter angle value > ";
                cin >> angle;
                if (!cin) {
                        cerr << "Invalid input" << endl;
                        break;
                }                       

                s.setAngle(angle);

                // write servo command to serial port
    unsigned char command[3];           
                command[0] = 0xff; //controller magic number
                command[1] = servoNum;
                command[2] = s.getValue();

                cout << "moving servo " << servoNum << " to " << s.getValue() << endl;
                int result = write(servo_fd, command, sizeof(command));      
                if (result == -1) {     
                        if (errno == EAGAIN) {
                                fprintf(stderr, "hwInterface write, would block writing servo values to serial port\n");
                        }
                        else {
                                perror("hwInterface error writing to serial port");
                        }
                        break;
                }

        } while (1);

        
        close(servo_fd);

        return 0;
}

---