Hello, I am having problems getting my maestro controller working and I can’t for the life of me can’t see why it isn’t working.
/**
* Date: 19/07/2010
*
**/
#include "Polulu_Interface.h"
int angles[6]={3000,3000,3000,3000,3000,3000}; // Starting positions for all servos
static int minservo=0, maxservo=254, maxspeed=127, maxaccel=255, error=0, safe=1, channel = 0x01; // Minimum and maximum of the servos, error and safety mode flag
HANDLE comPort;
/**
*
* This method establishes a connection with the Polulu controller.
* After a successful connection, the motion platform is centered.
*
* A integer is returned with the following significance:
* No error = 0
* Error = -1
**/
int Polulu_Interface::connect(int comPortNumber){
comPort=openPort(comPortNumber); // Polulu controller COM Port number
setSpeed(2); // Set speed to medium
neutral(); // Center the motion platform
waitMS(1000); // Wait 1 second
return error; // Return the error flag
} // connect
/**
*
* This method centers the motion platform and closes the
* connection with the Polulu controller.
*
**/
void Polulu_Interface::disconnect(){
setSpeed(2); // Set speed to medium
neutral(); // Center the motion platform
waitMS(2000); // Wait 2 second
servoOff(); // Shut all servos
CloseHandle(comPort); // Close connection to Polulu controller
system("PAUSE");
} // disconnect
/**
*
* This method sets the Polulu Interface in safe or unsafe mode.
* In safe mode (1) the actuators can only move between the 2500 and 3500 position (displacement of 1000).
* In unsafe mode (0) the actuators are free and can move between the 500 and 5500 position (displacement of 5000).
*
**/
void Polulu_Interface::safeMode(int s){
safe=s;
} //safeMode
/**
*
* This method is used to send a motion command to the motion platform.
* The inputs include the servo to be commanded (Servo 1 to 6),
* The speed at which it will move (Speed 1-100), and
* The absolute position to which it must move (1-1000 (safe mode), 1-5000 (unsafe mode))
*
* There is an input sanitization that prevents invalid commands from being executed
* depending on the mode of operation (safe, unsafe).
*
* Returns 0 for good command, -1 for invalid.
*
**/
int Polulu_Interface::motionCommand(int servo, int speed, int acceleration, int position){
// In safe mode
if(safe==1){
if( servo<1 || servo >6 || speed<1 || speed>100 || position <1 || position>1000)
return -1;
else{
setSpeed(servo-1, (int)speed*(maxspeed/100));
setAcceleration(servo-1, (int)acceleration*(maxaccel/100));
if(servo == 2 || servo == 4 || servo == 6){
if(position>500)
position=500-(position-500);
else if(position<500)
position=500+(500-position);
else if(position==500)
position=500;
} // if
moveActuator(servo-1, 1000-position+2500);
return 0;
} // else
} // if
// In unsafe mode
else if(safe==0){
if( servo<1 || servo >6 || speed<1 || speed>100 || position <1 || position>5000)
return -1;
else{
setSpeed(servo-1, (int)speed*(maxspeed/100));
setAcceleration(servo-1, (int)acceleration*(maxaccel/100));
if(servo == 2 || servo == 4 || servo == 6){
if(position>2500)
position=2500-(position-2500);
else if(position<2500)
position=2500+(2500-position);
else if(position==2500)
position=2500;
} // if
moveActuator(servo-1, 5000-position);
return 0;
} // else
} // else if
}// motionCommand
/**
*
* This method is used to get the position of
* the servos.
*
* Returns the current position of the servo.
*
**/
int Polulu_Interface::getPosition(int servo){
unsigned char buff[4];
int pos;
DWORD len;
buff[0]=0xAA; // start byte
buff[1]=channel; // device id
buff[2]=0x10; // command number
buff[3]=servo; // Servo number
pos=WriteFile(comPort, &buff, 4, &len, 0);
return pos;
}
/**
*
* This method is used privately to set three different speeds.
* It is possible to set the servos to the following speeds:
* 1 = low speed
* 2 = medium speed
* 3 = high speed
*
**/
void Polulu_Interface::setSpeed(int speed){
switch(speed){
case 1:
setSpeed(0, 1);
setSpeed(1, 1);
setSpeed(2, 1);
setSpeed(3, 1);
setSpeed(4, 1);
setSpeed(5, 1);
break;
case 2:
setSpeed(0, 20);
setSpeed(1, 20);
setSpeed(2, 20);
setSpeed(3, 20);
setSpeed(4, 20);
setSpeed(5, 20);
break;
case 3:
setSpeed(0, 127);
setSpeed(1, 127);
setSpeed(2, 127);
setSpeed(3, 127);
setSpeed(4, 127);
setSpeed(5, 127);
break;
default:
setSpeed(2);
break;
} // switch
} // setSpeed
/**
*
* This method is used privately to set all the servos to the neutral position.
*
**/
void Polulu_Interface::neutral(){
int i;
for(i=0;i<6;i++){
//moveActuator(i,(maxservo+minservo)/2);
}
}
/**
*
* Private method sending a Polulu mode command to move a servo
* to the desired position.
*
**/
void Polulu_Interface::moveActuator(int servo, int angle){
unsigned char buff[6];
DWORD len;
unsigned short int temp;
unsigned char pos_hi,pos_low;
temp=angle&0x1f80;
pos_hi=temp>>7;
pos_low=angle & 0x7f;
buff[0]=0xAA; //start byte
buff[1]=0x01; //device id
buff[2]=0x04; //command number
buff[3]=5; //servo number
buff[4]=pos_hi; //data1
buff[5]=pos_low;//data2
//buff[0] = 0x84; // Command byte: Set Target.
//buff[1] = servo; //channel; // First data byte holds channel number.
//buff[2] = angle & 0x7F; // Second byte holds the lower 7 bits of target.
//buff[3] = (angle >> 7) & 0x7F; // Third data byte holds the bits 7-13 of target.
WriteFile(comPort, &buff, 6, &len, 0);
}
/**
*
* Private method used to the set the speed of a servo using
* Polulu mode command.
*
**/
void Polulu_Interface::setSpeed(int servo, int speed){
unsigned char buff[5];
DWORD len;
buff[0]=0xAA; // start byte
buff[1]=channel; // device id
buff[2]=0x07; // command number
buff[3]=servo; // Servo number
if(speed > maxspeed) // Safety upper bound
speed = 0;
else if(speed < 1) // Safety lower bound
speed = 1;
buff[4]=speed;
WriteFile(comPort, &buff, 5, &len, 0);
}
/**
*
* Private method used to the set the acceleration of a
* servo using Polulu mode command.
*
**/
void Polulu_Interface::setAcceleration(int servo, int acceleration){
unsigned char buff[5];
DWORD len;
buff[0]=0xAA; // start byte
buff[1]=channel; // device id
buff[2]=0x09; // command number
buff[3]=servo; // Servo number
if(acceleration > maxaccel) // Safety upper bound
acceleration = 0;
else if(acceleration < 1) // Safety lower bound
acceleration = 1;
buff[4]=acceleration;
WriteFile(comPort, &buff, 5, &len, 0);
}
/**
*
* Private method used to shut down a servo using Polulu mode
* command.
*
**/
void Polulu_Interface::servoOff(){
unsigned char buff[6];
DWORD len;
int servo;
buff[0]=0xAA;//start byte
buff[1]=channel;//device id
buff[2]=0x04;//command number
buff[4]=0x00;
buff[5]=0x00;//off command
for(servo=0;servo<6;servo++) {
buff[3]=servo;//servo number
WriteFile(comPort, &buff, 6, &len, 0);
}
}
/**
*
* Method used to pause the execution for a set
* time in milliseconds.
*
**/
void Polulu_Interface::waitMS(int ms){
clock_t endwait;
endwait=clock()+ms*CLOCKS_PER_SEC/1000;
while(clock()<endwait);
}
/**
*
* Method used to open a serial interface communications port with the
* Polulu controller.
*
**/
HANDLE Polulu_Interface::openPort(int portnum){
char port[200]="com", pnum[]="Error";
itoa(portnum,pnum,10);
strcat(port,pnum);
HANDLE serial=CreateFile(port,GENERIC_READ|GENERIC_WRITE,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if(serial==INVALID_HANDLE_VALUE){
if(GetLastError()==ERROR_FILE_NOT_FOUND){
//printf("Error, %s Not Found\n", port);
error=-1;
return serial;
} // if
//printf("Com Error\n");
error=-1;
return serial;
} // if
DCB dcbSerialParams={0};
dcbSerialParams.DCBlength=sizeof(dcbSerialParams);
if(!GetCommState(serial, &dcbSerialParams)){
//printf("Com State Error\n");
error=-1;
return serial;
} // if
dcbSerialParams.BaudRate=CBR_19200;//CBR_baudrate
dcbSerialParams.ByteSize=8;
dcbSerialParams.Parity=NOPARITY;//NOPARITY, ODDPARITY, EVENPARITY
dcbSerialParams.StopBits=ONESTOPBIT;//ONESTOPBIT, ONE5STOPBITS, TWOSTOPBITS
if(!SetCommState(serial, &dcbSerialParams)){
//printf("Serial Protocol Error\n");
error=-1;
return serial;
} // if
COMMTIMEOUTS timeouts={0};
timeouts.ReadIntervalTimeout=50;
timeouts.ReadTotalTimeoutConstant=50;
timeouts.ReadTotalTimeoutMultiplier=10;
timeouts.WriteTotalTimeoutConstant=50;
timeouts.WriteTotalTimeoutMultiplier=10;
if(!SetCommTimeouts(serial,&timeouts)){
//printf("Timeout Setting Error\n");
error=-1;
return serial;
} // if
return serial;
} // openPort#ifndef POLULU_INTERFACE_H //header will only be defined if not already defined
#define POLULU_INTERFACE_H
/**
* Date: 19/07/2010
* Version: 1.0
*
* Polulu_Interface.h
*
**/
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <conio.h>
#include <time.h>
class Polulu_Interface{
public:
int connect(int comPortNumber);
void disconnect();
void safeMode(int s);
int motionCommand(int servo, int speed, int acceleration, int position);
int getPosition(int servo);
void waitMS(int ms); // Milisecond delay function
private:
HANDLE openPort(int);
void moveActuator(int servo, int angle); // Moves servo (0-5) to angle (500-5500)
void setSpeed(int servo, int speed); // Set the speed of one serve (1-127)
void setSpeed(int speed); // 1 = low, 2 = medium, 3 = high
void setAcceleration(int servo, int acceleration); // Set the acceleration of one servo
void servoOff(); // Stops signal to all servos, analog servos will go slack, digital servos wont!
void neutral(); // Sends all servos to halfway between max and min
}; // Polulu_Interface
#endif // POLULU_INTERFACE_H