Pololu Robotics & Electronics
Menu
My account Comments or questions? About Pololu Contact Ordering information Distributors

Micro Maestro 6-Channel USB


#1

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.

[code]/**

  • 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[/code]

[code]
#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[/code]


#2

Hello,

Have you read our article Read Before You Post: General Advice for Getting Help that is at the top of every forum? It is impossible for anyone to help you based on what you posted.

-Paul