143 lines
3.2 KiB
C++
143 lines
3.2 KiB
C++
#include "stepper.h"
|
|
|
|
Stepper::Stepper(QObject *parent) : QObject(parent)
|
|
{
|
|
isOpen = 0;
|
|
}
|
|
|
|
Stepper::~Stepper()
|
|
{
|
|
disconnect();
|
|
}
|
|
|
|
//******** Basic IO *******
|
|
|
|
int Stepper::command(int command, int type, unsigned int parameter, unsigned int* param_out)
|
|
{
|
|
//1. Prepare data
|
|
unsigned char buffer[9];
|
|
buffer[0] = (unsigned char)moduleAddress;
|
|
buffer[1] = (unsigned char)command;
|
|
buffer[2] = (unsigned char)type;
|
|
buffer[3] = 0; //Motor/bank number
|
|
for (int i = 0; i < 4; i++)
|
|
buffer[4+i] = (unsigned char)(parameter >> (8*(3-i)));
|
|
buffer[8] = 0; //checksum
|
|
for (int i = 0; i < 8; i++)
|
|
buffer[8] += buffer[i]; //calculate checksum
|
|
|
|
//2. Send data
|
|
serialPort.write((const char*)buffer, 9);
|
|
serialPort.waitForBytesWritten(500);
|
|
|
|
//3. Read data
|
|
int ptr = 0;
|
|
while (1)
|
|
{
|
|
if (serialPort.read((char*)buffer+ptr,1) <= 0)
|
|
{
|
|
if (!serialPort.waitForReadyRead(500))
|
|
break; //no new data - break
|
|
}
|
|
else
|
|
{
|
|
ptr++;
|
|
if (ptr >= 9)
|
|
break; //buffer full = break
|
|
}
|
|
}
|
|
|
|
//4. Parse data
|
|
if (ptr < 9)
|
|
return STEPPER_ERROR_NOREPLY; //no new data
|
|
if (buffer[2] != 100)
|
|
return STEPPER_ERROR_CHECKSUM;
|
|
|
|
if (param_out != NULL)
|
|
{
|
|
*param_out = ( (unsigned int)buffer[4]<<24 ) |
|
|
( (unsigned int)buffer[5]<<16 ) |
|
|
( (unsigned int)buffer[6]<<8 ) |
|
|
( (unsigned int)buffer[7]);
|
|
}
|
|
return STEPPER_OK;
|
|
|
|
}
|
|
|
|
//******** Commands *********
|
|
|
|
int Stepper::command_ROL(int speed)
|
|
{
|
|
return command(2,0,speed);
|
|
}
|
|
|
|
int Stepper::command_ROR(int speed)
|
|
{
|
|
return command(1,0,speed);
|
|
}
|
|
|
|
int Stepper::command_MST()
|
|
{
|
|
return command(3,0,0);
|
|
}
|
|
|
|
int Stepper::command_MVP(int ref_type, int position)
|
|
{
|
|
return command(4, ref_type, position);
|
|
}
|
|
|
|
int Stepper::command_SAP(int parameter_number, int value)
|
|
{
|
|
return command(5, parameter_number, value);
|
|
}
|
|
|
|
int Stepper::command_GAP(int parameter_number, int* value)
|
|
{
|
|
return command(6, parameter_number, 0, (unsigned int*)value);
|
|
}
|
|
|
|
//******** Connecting ***********
|
|
|
|
int Stepper::connect(int address)
|
|
{
|
|
disconnect(); //just if we were connected
|
|
|
|
moduleAddress = address;
|
|
|
|
QList<QSerialPortInfo> list = QSerialPortInfo::availablePorts();
|
|
|
|
if (list.size() == 0)
|
|
return -1; //no serial ports found
|
|
|
|
for (int port = 0; port < list.size(); port++)
|
|
{
|
|
serialPort.setPort(list[port]);
|
|
serialPort.setBaudRate(9600);
|
|
if (serialPort.open(QIODevice::ReadWrite))
|
|
{
|
|
serialPort.setBaudRate(9600);
|
|
|
|
if (command_MST() == STEPPER_OK)
|
|
{ //gotta our PS!
|
|
isOpen = 1;
|
|
portName = list[port].portName();
|
|
//maybe configure something
|
|
command_SAP(STEPPER_SAP_MAXIMUM_SPEED,1000);
|
|
|
|
break;
|
|
}
|
|
serialPort.close();
|
|
}
|
|
}
|
|
return isOpen;
|
|
}
|
|
|
|
void Stepper::disconnect()
|
|
{
|
|
if (!isOpen)
|
|
return;
|
|
|
|
serialPort.close();
|
|
isOpen = 0;
|
|
}
|