GPSBots: Linux Tutorial: Pololu Motor & Servo Controllers

Linux Tutorial: Pololu Motor & Servo Controllers

Pololu Motor and Servo Controllers

Pololu makes two great products that make controlling a robot from a computer incredibly easy. By sending a few bytes out on the serial port you can move a servo to a position or set the speed of a motor. The code here shows how to properly send those packets to the serial port.

Please see the IO Class Tutorial for the oIO object code.

Usage: SetMotor(MOTOR, 40); to set motor # MOTOR to 40%

Usage: SetServo(SERVO, 85); move servo to position 85%

To compile: g++ pololu.cpp io.cpp

pololu.cpp

using namespace std;
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include "io.h"
#define SERIALPORT "/dev/ttyS0"
#define BAUDRATE 4800
#define SERVO 7      // servo's pololu #
#define MOTOR 8      // the motor's pololu #
#define FULL_SPEED 127      // actual value to send to the motor to go full speed (- is full backwards)
#define MIN_SPEED 25      // minimum stall speed
#define FORWARD 0      // forward polarity (depends on motor wiring)
#define BACKWARD 1      // opposite of FORWARD

bool SetMotor(int num, double percent);
bool SetServo(int num, double percent);
IO oIO; // generic IO object

void main() {
SetMotor(MOTOR, 50);
SetServo(SERVO, 45);
}

bool SetMotor(int num, double percent) {
          // :: Convert to an actual value, then send down the serial line the motor control codes ::
     if (percent < -100 || percent > 100) {
          return false;
     } else {
          int direction = FORWARD;
          double new_speed = ((FULL_SPEED - MIN_SPEED) * (percent/100.0));
     //           if (percent > 0) { new_speed += MIN_SPEED; } else { new_speed -= MIN_SPEED; } / / adjust for stall speed
          if (percent < 0) {
               direction = BACKWARD;
          }
          unsigned char out[4];
          out[0] = 0x80;      // start byte
          out[1] = 0x00;      // deviceid (0=motor)
          out[2] = (num*2)+direction;      // msb(7) = motor num, lsb = direction
     // cout << "percent=" << percent << "; dir=" << direction << "; out2=" << (int)out[2] << ";" << endl;
          out[3] = (int)abs((int)new_speed);      // max 0x7f
          oIO.SendSerial(out, 4);
     }
     return true;
}

bool SetServo(int num, double percent) {
          // :: Set the position of a servo ::
     if (percent < 0 || percent > 100) {
          return false;
     } else {
          unsigned char out[6];
          out[0] = 0x80;      // start byte
          out[1] = 0x01;      // deviceid (1=servo)
          out[2] = 0x04;      // 2=7bit position
          out[3] = num;
     // min: 0x0374 max: 0x4a7c
          out[4] = (char)(0x03 + ((0x4a - 0x03) * (percent/100.0)));      // max 0x03-0x4a
          out[5] = 0x74;
               //out[4] = 0x03;
               //out[5] = 0x74;
          oIO.SendSerial(out, 6);
     }
     return true;
}