mirror of
https://github.com/isaac879/Pan-Tilt-Mount.git
synced 2026-08-16 13:03:15 +02:00
418 lines
16 KiB
C++
418 lines
16 KiB
C++
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
/*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
* IN THE SOFTWARE
|
|
*
|
|
* Code written by isaac879
|
|
*/
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
#include <stdio.h>
|
|
#include <Windows.h>
|
|
#include <Xinput.h>
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <fstream>
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
//#pragma comment(lib, "XInput.lib") // Library. If your compiler doesn't support this type of lib include change to the corresponding one
|
|
|
|
#define UP_BUTTON 1
|
|
#define DOWN_BUTTON 2
|
|
#define LEFT_BUTTON 4
|
|
#define RIGHT_BUTTON 8
|
|
#define MENU_BUTTON 16
|
|
#define VIEW_BUTTON 32
|
|
#define L_BUTTON 64
|
|
#define R_BUTTON 128
|
|
#define LB_BUTTON 256
|
|
#define RB_BUTTON 512
|
|
#define A_BUTTON 4096
|
|
#define B_BUTTON 8192
|
|
#define X_BUTTON 16384
|
|
#define Y_BUTTON 32768
|
|
|
|
#define INSTRUCTION_BYTES_PAN_SPEED 1
|
|
#define INSTRUCTION_BYTES_TILT_SPEED 2
|
|
#define INSTRUCTION_BYTES_PAN_TILT_SPEED 3
|
|
#define INSTRUCTION_BYTES_SLIDER_PAN_TILT_SPEED 4
|
|
|
|
#define INPUT_DEADZONE 4000
|
|
|
|
#define MAXIMUM_PAN_STEP_SPEED 1130.0 //steps per second
|
|
#define MAXIMUM_TILT_STEP_SPEED 410.0
|
|
#define MAXIMUM_SLIDER_STEP_SPEED 900.0
|
|
|
|
//triggers are 8 bit
|
|
//analogsticks are 16bit
|
|
|
|
//Flags
|
|
bool portOpenFlag = false; //The flag used to determin if the serial port for the Arduino Nano is opened
|
|
HANDLE hSerial;// Serial handle
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
byte getBytesFromInt(short sourceInt, unsigned int byteNumber) {//Gets a byte from an int. byte number should be 0 for the 8 least signigicant bits or 1 for the most fignificant bits
|
|
byte byteValue = (sourceInt >> (8 * byteNumber)) & 0xFF;
|
|
return byteValue;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
void openSerial(LPCSTR port){
|
|
if (portOpenFlag == false){//If the port is not already open
|
|
hSerial = CreateFile(port, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);//Set the Com port to the correct arduino one
|
|
|
|
if (hSerial == INVALID_HANDLE_VALUE){
|
|
std::cout << "Error: invalid com port handle. Attempted to open: " << port << std::endl;
|
|
}
|
|
else{
|
|
portOpenFlag = true;
|
|
std::cout << "Successfully opened: " << port << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int closeSerial(){
|
|
if (portOpenFlag == true){//If the port is open
|
|
if (CloseHandle(hSerial) == 0){
|
|
std::cout << "Error\n" << stderr << std::endl;
|
|
return -1;
|
|
}
|
|
portOpenFlag = false;
|
|
printf("Serial port closed...\n");
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int serialConnect(LPCSTR portName){
|
|
std::cout << "Attempting to open serial port " << portName << std::endl;
|
|
openSerial(portName);//Open the serial port
|
|
|
|
// Declare variables and structures
|
|
DCB dcbSerialParams = { 0 };
|
|
dcbSerialParams.DCBlength = sizeof(dcbSerialParams);//why do i need this?
|
|
if (GetCommState(hSerial, &dcbSerialParams) == 0){
|
|
closeSerial();
|
|
return -1;
|
|
}
|
|
else{
|
|
dcbSerialParams.BaudRate = CBR_57600; //Set device parameters (57600 baud, 1 start bit, 1 stop bit, no parity)
|
|
dcbSerialParams.ByteSize = 8;
|
|
dcbSerialParams.StopBits = ONESTOPBIT;
|
|
dcbSerialParams.Parity = NOPARITY;
|
|
dcbSerialParams.fDtrControl = DTR_CONTROL_DISABLE; //Stops the Arduino resetting after connecting
|
|
}
|
|
|
|
if(SetCommState(hSerial, &dcbSerialParams) == 0){
|
|
closeSerial();
|
|
return -1;
|
|
}
|
|
|
|
// Set COM port timeout settings
|
|
COMMTIMEOUTS timeouts = { 0 };
|
|
timeouts.ReadIntervalTimeout = 50;
|
|
timeouts.ReadTotalTimeoutConstant = 50;
|
|
timeouts.ReadTotalTimeoutMultiplier = 10;
|
|
timeouts.WriteTotalTimeoutConstant = 50;
|
|
timeouts.WriteTotalTimeoutMultiplier = 10;
|
|
|
|
if(SetCommTimeouts(hSerial, &timeouts) == 0){
|
|
closeSerial();
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int serialWrite(char data[], int lengthOfArray){
|
|
DWORD bytes_written;
|
|
if (!WriteFile(hSerial, data, lengthOfArray, &bytes_written, NULL)) {
|
|
closeSerial();
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
void printIncomingData(void){
|
|
COMSTAT status;
|
|
DWORD errors;
|
|
DWORD bytesRead;//Number of bytes that have been read
|
|
|
|
while(1){
|
|
ClearCommError(hSerial, &errors, &status); //Use the ClearCommError function to get status info on the Serial port
|
|
if(status.cbInQue == 0) break;
|
|
|
|
char* dataBuffer = new char[status.cbInQue + 1]; //buffer the size of the data available to be read
|
|
dataBuffer[status.cbInQue] = '\0'; //adds a null terminator to the end of the buffer
|
|
|
|
if(ReadFile(hSerial, dataBuffer, status.cbInQue, &bytesRead, NULL)){ //Try to read the require number of chars, and return the number of read bytes on success
|
|
std::cout << dataBuffer;// << std::endl;
|
|
}
|
|
delete[] dataBuffer;
|
|
Sleep(5); //Wait for 2ms before checking if more data has come in
|
|
}
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
void sendCommand(char command){
|
|
char data[] = {command}; //Data array to send
|
|
serialWrite(data, sizeof(data));
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
void sendCharArray(char *array){
|
|
serialWrite(array, (int)strlen(array));
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int sendStepSpeed(int command, short num){
|
|
char data[3]; //Data array to send
|
|
|
|
data[0] = command;
|
|
data[1] = getBytesFromInt(num, 1);//Gets the 8 MSBs
|
|
data[2] = getBytesFromInt(num, 0);//Gets the 8 LSBs
|
|
|
|
serialWrite(data, sizeof(data)); //Send the command and the 4 bytes of data
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int sendPanTiltStepSpeed(int command, int num){
|
|
char data[5]; //Data array to send
|
|
|
|
data[0] = command;
|
|
data[1] = (num >> (8 * 3)) & 0xFF; //Gets the most significant byte
|
|
data[2] = (num >> (8 * 2)) & 0xFF; //Gets the second most significant byte
|
|
data[3] = (num >> (8 * 1)) & 0xFF; //Gets the third most significant byte
|
|
data[4] = num & 0xFF; //Gets the least significant byte
|
|
|
|
serialWrite(data, sizeof(data)); //Send the command and the 4 bytes of data
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int sendSliderPanTiltStepSpeed(int command, short *arr){
|
|
char data[7]; //Data array to send
|
|
|
|
data[0] = command;
|
|
data[1] = (arr[0] >> 8) & 0xFF; //Gets the most significant byte
|
|
data[2] = arr[0] & 0xFF; //Gets the second most significant byte
|
|
data[3] = (arr[1] >> 8) & 0xFF;
|
|
data[4] = arr[1] & 0xFF;
|
|
data[5] = (arr[2] >> 8) & 0xFF;
|
|
data[6] = arr[2] & 0xFF; //Gets the least significant byte
|
|
|
|
serialWrite(data, sizeof(data)); //Send the command and the 6 bytes of data
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
std::string getPortName(std::string filePath){
|
|
std::string line;
|
|
std::cout << "Reading port name from: " << filePath << std::endl;
|
|
std::ifstream readfile(filePath);
|
|
|
|
if(readfile.is_open()){
|
|
if (getline(readfile, line)){
|
|
std::cout << "Port read from the file: " << line << std::endl;
|
|
}
|
|
readfile.close();
|
|
}
|
|
else {
|
|
std::cout << "Error: Unable to open serial_port.txt" << std::endl;
|
|
}
|
|
return line;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int panTiltInit(void){
|
|
//Connects to the com port that the Arduino is connected to.
|
|
for(int i = 0; (i < 5) && (serialConnect(getPortName("D:\\Documents\\Projects\\Pan Tilt Mount\\serial_port.txt").c_str()) != 0 ); i++){ //path to .txt files containing the name of the COM port. The file should contain "\\.\COM10" without quotes to connect to COM10
|
|
Sleep(1000);
|
|
if(i == 4){
|
|
std::cout << "Error: Unable to open serial port after 5 attempts..." << std::endl;
|
|
return -1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
int main(void){
|
|
panTiltInit();
|
|
short shortVals[3] = {0, 0, 0};
|
|
DWORD dwResult;
|
|
WORD lastwButtons = 0;
|
|
DWORD lastDwPacketNumber = 0;
|
|
for(DWORD i = 0; i < XUSER_MAX_COUNT; i++){
|
|
XINPUT_STATE state;
|
|
ZeroMemory(&state, sizeof(XINPUT_STATE));
|
|
|
|
dwResult = XInputGetState(i, &state); //Simply get the state of the controller from XInput.
|
|
if(dwResult == ERROR_SUCCESS){ //Controller is connected
|
|
printf("Connected.\n");
|
|
while(1){
|
|
if (GetKeyState(VK_ESCAPE) & 0x8000) { //MSB is key state (1 if pressed). LSB is the toggle state
|
|
break;
|
|
}
|
|
if (GetKeyState(VK_F8) & 0x8000) {
|
|
char data[10];
|
|
std::cin.getline(data, 10);
|
|
std::cin.clear();
|
|
serialWrite(data, sizeof(data));
|
|
}
|
|
printIncomingData();
|
|
XInputGetState(i, &state);
|
|
//printf("dw packet number: %d\n", state.dwPacketNumber);
|
|
if(state.dwPacketNumber != lastDwPacketNumber){
|
|
short RXShort = 0;
|
|
short RYShort = 0;
|
|
short LXShort = 0;
|
|
float RX = state.Gamepad.sThumbRX; //Get right analog stick X value
|
|
float RY = state.Gamepad.sThumbRY; //Get right analog stick Y value
|
|
float LX = state.Gamepad.sThumbLX; //Get left analog stick X value
|
|
float LY = state.Gamepad.sThumbLY; //Get left analog stick Y value
|
|
float magnitude = sqrt(RX * RX + RY * RY);
|
|
float magnitudeL = sqrt(LX * LX + LY * LY);
|
|
|
|
float normalizedMagnitude = 0;
|
|
|
|
if(magnitude > INPUT_DEADZONE){ //check if the controller is outside a circular dead zone
|
|
if(magnitude > 32767){ //clip the magnitude at its expected maximum value
|
|
magnitude = 32767;
|
|
}
|
|
|
|
normalizedMagnitude = magnitude / (32767 - INPUT_DEADZONE);
|
|
float scale = normalizedMagnitude / magnitude;
|
|
RXShort = -((RX * scale) * abs(RX * scale) * MAXIMUM_PAN_STEP_SPEED);
|
|
RYShort = -((RY * scale) * abs(RY * scale) * MAXIMUM_TILT_STEP_SPEED);
|
|
}
|
|
else{
|
|
RXShort = 0;
|
|
RYShort = 0;
|
|
}
|
|
|
|
float normalizedMagnitudeL = 0;
|
|
if(magnitudeL > INPUT_DEADZONE){ //check if the controller is outside a circular dead zone
|
|
if(magnitudeL > 32767){ //clip the magnitude at its expected maximum value
|
|
magnitudeL = 32767;
|
|
}
|
|
|
|
normalizedMagnitudeL = magnitudeL / (32767 - INPUT_DEADZONE);
|
|
float scale = normalizedMagnitudeL / magnitudeL;
|
|
LXShort = (LX * scale) * abs(LX * scale) * MAXIMUM_SLIDER_STEP_SPEED;
|
|
}
|
|
else{
|
|
LXShort = 0;
|
|
}
|
|
|
|
shortVals[0] = LXShort;
|
|
printf("LXShort: %d \n", LXShort);
|
|
shortVals[1] = RXShort;
|
|
printf("RXShort: %d \n", RXShort);
|
|
shortVals[2] = RYShort;
|
|
printf("RYShort: %d \n", RYShort);
|
|
|
|
sendSliderPanTiltStepSpeed(INSTRUCTION_BYTES_SLIDER_PAN_TILT_SPEED, shortVals); //send the combned values
|
|
Sleep(10);
|
|
|
|
if((lastwButtons & UP_BUTTON) < (state.Gamepad.wButtons & UP_BUTTON)){
|
|
printf("Up: %d \n", (state.Gamepad.wButtons & UP_BUTTON));
|
|
sendCharArray((char *)"@"); //Up first element
|
|
}
|
|
if((lastwButtons & DOWN_BUTTON) < (state.Gamepad.wButtons & DOWN_BUTTON)){
|
|
printf("Down: %d \n", ((state.Gamepad.wButtons & UP_BUTTON) >> 1));
|
|
sendCharArray((char *)"Z"); //down last element
|
|
}
|
|
if((lastwButtons & LEFT_BUTTON) < (state.Gamepad.wButtons & LEFT_BUTTON)){
|
|
printf("Left: %d \n", ((state.Gamepad.wButtons & LEFT_BUTTON) >> 2));
|
|
sendCharArray((char *)"<"); //left step back
|
|
}
|
|
if((lastwButtons & RIGHT_BUTTON) < (state.Gamepad.wButtons & RIGHT_BUTTON)){
|
|
printf("Right: %d \n", ((state.Gamepad.wButtons & RIGHT_BUTTON) >> 3));
|
|
sendCharArray((char *)">"); //right step forwards
|
|
}
|
|
if((lastwButtons & MENU_BUTTON) < (state.Gamepad.wButtons & MENU_BUTTON)){
|
|
printf("Menu: %d \n", ((state.Gamepad.wButtons & MENU_BUTTON) >> 4));
|
|
sendCharArray((char *)"A"); //Menu home
|
|
}
|
|
if((lastwButtons & VIEW_BUTTON) < (state.Gamepad.wButtons & VIEW_BUTTON)){
|
|
printf("View: %d \n", ((state.Gamepad.wButtons & VIEW_BUTTON) >> 5));
|
|
sendCharArray((char *)";1"); //views execute
|
|
}
|
|
if((lastwButtons & L_BUTTON) < (state.Gamepad.wButtons & L_BUTTON)){
|
|
printf("L: %d \n", ((state.Gamepad.wButtons & L_BUTTON) >> 6));
|
|
sendCharArray((char *)"m2"); //half step mode
|
|
}
|
|
if((lastwButtons & R_BUTTON) < (state.Gamepad.wButtons & R_BUTTON)){
|
|
printf("R: %d \n", ((state.Gamepad.wButtons & R_BUTTON) >> 7));
|
|
sendCharArray((char *)"m16"); //sixteenth step mode
|
|
}
|
|
if((lastwButtons & LB_BUTTON) < (state.Gamepad.wButtons & LB_BUTTON)){
|
|
printf("LB: %d \n", ((state.Gamepad.wButtons & LB_BUTTON) >> 8));
|
|
sendCharArray((char *)"D300"); //add delay of 300ms
|
|
}
|
|
if((lastwButtons & RB_BUTTON) < (state.Gamepad.wButtons & RB_BUTTON)){
|
|
printf("RB: %d \n", ((state.Gamepad.wButtons & RB_BUTTON) >> 9));
|
|
sendCharArray((char *)"c"); //RB Shutter
|
|
}
|
|
if((lastwButtons & A_BUTTON) < (state.Gamepad.wButtons & A_BUTTON)){
|
|
printf("A: %d \n", ((state.Gamepad.wButtons & A_BUTTON) >> 12));
|
|
sendCharArray((char *)"#"); //A save position
|
|
}
|
|
if((lastwButtons & B_BUTTON) < (state.Gamepad.wButtons & B_BUTTON)){
|
|
printf("B: %d \n", ((state.Gamepad.wButtons & B_BUTTON) >> 13));
|
|
sendCharArray((char *)"C"); //B clear array
|
|
}
|
|
if((lastwButtons & X_BUTTON) < (state.Gamepad.wButtons & X_BUTTON)){
|
|
printf("X: %d \n", ((state.Gamepad.wButtons & X_BUTTON) >> 14));
|
|
sendCharArray((char *)"E"); //X edit position
|
|
}
|
|
if((lastwButtons & Y_BUTTON) < (state.Gamepad.wButtons & Y_BUTTON)){
|
|
printf("Y: %d \n", ((state.Gamepad.wButtons & Y_BUTTON) >> 15));
|
|
sendCharArray((char *)"R"); //Y status
|
|
}
|
|
//printf("LX: %d \t", state.Gamepad.sThumbLX);
|
|
//printf("LY: %d \t", state.Gamepad.sThumbLY);
|
|
//printf("RX: %d \t", state.Gamepad.sThumbRX);
|
|
//printf("RY: %d \n", state.Gamepad.sThumbRY);
|
|
//printf("LT: %d \t", state.Gamepad.bLeftTrigger);
|
|
//printf("RT: %d \n", state.Gamepad.bRightTrigger);
|
|
//printf("wButtons %d \n", state.Gamepad.wButtons);
|
|
lastwButtons = state.Gamepad.wButtons;
|
|
lastDwPacketNumber = state.dwPacketNumber;
|
|
}
|
|
}
|
|
}
|
|
else{ //Controller is not connected
|
|
printf("Not Connected.\n");
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------------------------------------------------------------------------------------*/
|