00001 /* 00002 * ControlComm.cpp 00003 * 00004 * Created on: Apr 26, 2009 00005 * Author: jgoppert 00006 */ 00007 00008 #include "ControlComm.h" 00009 #include <iostream> 00010 #include "uvsim/utilities/utilities.h" 00011 #include <jsw.h> 00012 #include <cstdlib> 00013 #include <cmath> 00014 00015 namespace uvsim 00016 { 00017 00018 ControlComm::ControlComm(std::string servoPort, bool useJoystick) 00019 { 00020 // setup serial port 00021 m_serialPort.Open(servoPort); 00022 m_serialPort.SetBaudRate(SerialStreamBuf::BAUD_115200); 00023 m_serialPort.SetCharSize(SerialStreamBuf::CHAR_SIZE_8); 00024 m_serialPort.SetParity(SerialStreamBuf::PARITY_NONE); 00025 m_serialPort.SetNumOfStopBits(1); 00026 m_serialPort.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE); 00027 00028 // check if serial port setup correctly 00029 if (!m_serialPort.good()) 00030 { 00031 std::cerr << "Error: Control Serial port not setup correctly." << std::endl; 00032 exit(1); 00033 } 00034 00035 // joystick 00036 m_useJoystick = useJoystick; 00037 if (m_useJoystick) 00038 { 00039 m_autoMode = false; 00040 if (!fileExists(JSDefaultDevice)) 00041 std::cout << "Warning: joystick not found!." << std::endl; 00042 if (!fileExists(joystickCalibration)) 00043 { 00044 std::cout << "Warning: " << joystickCalibration 00045 << " not found. Please run jscalibrator program and save to " 00046 << joystickCalibration << "." << std::endl; 00047 exit(1); 00048 } 00049 JSInit(&m_joystick, JSDefaultDevice, joystickCalibration, 00050 JSFlagNonBlocking); 00051 m_joystick.axis[3]->correction_level = 1; 00052 m_joystick.axis[0]->correction_level = 1; 00053 std::cout << "Control thread initialized." << std::endl; 00054 } 00055 } 00056 00057 ControlComm::~ControlComm() 00058 { 00059 if (m_useJoystick) 00060 JSClose(&m_joystick); 00061 m_serialPort.Close(); 00062 } 00063 00064 void ControlComm::update() 00065 { 00066 std::cout << "\nControl thread running." << std::endl; 00067 00068 if (m_useJoystick) 00069 { 00070 JSUpdate(&m_joystick); 00071 float throttle, steering, headingAngleFeedback; 00072 m_throttle = JSGetAxisCoeffNZ(&m_joystick, 3); 00073 m_steering = JSGetAxisCoeffNZ(&m_joystick, 0); 00074 m_autoMode = JSGetButtonState(&m_joystick, 0); 00075 00076 std::cout << "throttle: " << m_throttle << std::endl; 00077 std::cout << "steering: " << m_steering << std::endl; 00078 std::cout << "autoMode: " << m_autoMode << std::endl; 00079 m_headingAngleFeedback = 0.0; 00080 00081 } 00082 else 00083 { 00084 m_throttle = 0; 00085 m_steering = 0; 00086 m_headingAngleFeedback = 0; 00087 } 00088 00089 m_msg[0] = round(m_throttle*127); 00090 m_msg[1] = round(m_steering*127); 00091 m_msg[2] = round(m_headingAngleFeedback*127); 00092 00093 sum.sint = m_msg[0] + m_msg[1] + m_msg[2]; // checksum 00094 00095 std::cout << (int) m_msg[0] << " " << (int) m_msg[1] << " " << (int) m_msg[2] << " " << (int) sum.byte[0] << (int) sum.byte[1] << " " << sum.sint << std::endl; 00096 00097 // Send message to arduino 00098 m_serialPort << m_msgHeader << m_msg[0] << m_msg[1] << m_msg[2] << sum.byte[0] << sum.byte[1] << m_msgFooter << std::endl; 00099 } 00100 00101 }
1.6.1