void _test_websocket_server::send_msg(const test_websocket_msg& msg)
{
// Wait for the websocket server to be initialized.
pplx::task<void>(m_server_connected).wait();
const auto& data = msg.data();
auto flags = websocketpp::frame::opcode::close;
switch (msg.msg_type())
{
case test_websocket_message_type::WEB_SOCKET_UTF8_MESSAGE_TYPE:
flags = websocketpp::frame::opcode::text; // WebSocket::FRAME_FLAG_FIN | WebSocket::FRAME_OP_TEXT;
break;
case test_websocket_message_type::WEB_SOCKET_BINARY_MESSAGE_TYPE:
flags = websocketpp::frame::opcode::binary; // WebSocket::FRAME_FLAG_FIN | WebSocket::FRAME_OP_BINARY;
break;
case test_websocket_message_type::WEB_SOCKET_CLOSE_TYPE:
flags = websocketpp::frame::opcode::close; // WebSocket::FRAME_OP_CLOSE;
break;
case test_websocket_message_type::WEB_SOCKET_UTF8_FRAGMENT_TYPE:
case test_websocket_message_type::WEB_SOCKET_BINARY_FRAGMENT_TYPE:
default:
throw std::runtime_error("invalid message type");
}
std::string strmsg(data.begin(), data.end());
if (msg.msg_type() == test_websocket_message_type::WEB_SOCKET_CLOSE_TYPE)
{
close(strmsg);
}
else
{
// std::cerr << "Sending message from server: " << strmsg << std::endl;
m_srv.send(m_con, strmsg, flags);
}
}
void * speedometer(void * argument) {
const timespec waitTime = { 0, MOUSE_SAMPLE_RATE };
cout << waitTime.tv_nsec << " " << waitTime.tv_sec << endl;
int fd;
struct input_event ie;
if ((fd = open(MOUSE_FILE, O_RDONLY)) == -1) {
perror("opening device");
// exit(1);
}
int flags = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, flags | O_NONBLOCK);
int counter = 0;
while (true) {
//cout << "hello from the speedometer" << endl;
nanosleep(&waitTime, NULL);
//cout << "wait over" << endl;
counter = 0;
while (read(fd, &ie, sizeof(struct input_event)) != -1) {
//cout << "reading mouse" << endl;
if ((ie.type == 2) && (ie.code == 8)) {
counter++;
// cout << "new mouse event " << counter << endl;
}
}
//cout << "aquireing mutex" << endl;
//cout << "aquired mutex" << endl;
currentState.speed = (counter * MOUSE_MM_PER_CLICK) * 10; //mm/s
currentState.distanceTraveled = counter * MOUSE_MM_PER_CLICK; //mm.
//cout << "speed: " << currentState.speed << " distance: " << currentState.distanceTraveled << endl;
ptree answerJson;
stringstream jsonWriteStream;
answerJson.put("speed", currentState.speed);
answerJson.put("distanceTraveled", currentState.distanceTraveled);
write_json(jsonWriteStream, answerJson);
//cout << "sending speedometer" << endl;
//pthread_mutex_lock(&watchDog_mutex);
if (currentState.connected) {
websocket_server.send(currentState.client, jsonWriteStream.str(),
websocketpp::frame::opcode::text);
}
//pthread_mutex_unlock(&watchDog_mutex);
//cout << "released mutex" << endl;
}
return 0;
}
void on_message(websocketpp::connection_hdl hdl, server::message_ptr msg) {
try {
//cout << "message" << endl;
//immediatly send back a message for timing purposes
//websocket_server.send(hdl ,"TIME",websocketpp::frame::opcode::text);
//parse the json
ptree parsedJson;
stringstream jsonStream;
jsonStream << msg->get_payload();
read_json(jsonStream, parsedJson);
if (parsedJson.get<bool>("triangle")) {
cout << "Received triangle" << endl;
shutDown();
}
//toggle autonomous mode
if (parsedJson.get<bool>("start")) {
pthread_mutex_lock(&autonomous_mutex);
if (currentState.autonomous == true) {
currentState.autonomous = false;
cout << "Autonomous mode off." << endl;
} else {
currentState.autonomous = true;
cout << "Autonomous mode on." << endl;
}
pthread_mutex_unlock(&autonomous_mutex);
}
pthread_mutex_lock(&autonomous_mutex);
if (currentState.autonomous != true) {
//update the servo values
currentState.servoInt = (int) (SERVO_CENTER
+ (parsedJson.get<double>("leftAnalog_x")
* MAX_STEERING_OFFSET));
if (parsedJson.get<double>("rightAnalog_y") < 0) {
currentState.cruise = false;
}
if (currentState.cruise == false) {
//skip the center range
int requestedMotorInt = 0;
pthread_mutex_lock(&obsticle_mutex);
if ((parsedJson.get<double>("rightAnalog_y") > 0)
&& (currentState.obsticle != true)) {
//forward
//cout << "rightAnalog_y: " << parsedJson.get<double>("rightAnalog_y") << endl;
requestedMotorInt = (int) (parsedJson.get<double>(
"rightAnalog_y") * MOTOR_FORWARD_RANGE);
//cout << "requestedMotirInt(forward): " << requestedMotorInt << endl;
if ((requestedMotorInt - currentState.lastRequestedMotorInt)
> ACCELERATION_THRESHOLD) {
requestedMotorInt = currentState.lastRequestedMotorInt
+ ACCELERATION_THRESHOLD;
//cout << "Don't accelerate so quickly!" << endl;
}
currentState.motorInt = MOTOR_FORWARD_START
+ requestedMotorInt;
} else if (parsedJson.get<double>("rightAnalog_y") < 0) {
//backward
//cout << "rightAnalog_y: " << parsedJson.get<double>("rightAnalog_y") << endl;
requestedMotorInt = -1
* (int) (parsedJson.get<double>("rightAnalog_y")
* MOTOR_BACKWARD_RANGE);
//cout << "requestedMotirInt(backward): " << requestedMotorInt << endl;
if ((currentState.lastRequestedMotorInt - requestedMotorInt)
> ACCELERATION_THRESHOLD) {
requestedMotorInt = currentState.lastRequestedMotorInt
- ACCELERATION_THRESHOLD;
//cout << "Don't accelerate (backwards) so quickly!" << endl;
}
currentState.motorInt = MOTOR_BACKWARD_START
- requestedMotorInt;
if (currentState.cruise) {
currentState.cruise = false;
// cout << "Cruise mode off." << endl;
}
} else {
//stop
currentState.motorInt = SERVO_CENTER;
}
pthread_mutex_unlock(&obsticle_mutex);
//cout << "updating lastRequestedMotorInt: " << currentState.lastRequestedMotorInt << endl;
currentState.lastRequestedMotorInt = requestedMotorInt;
//update motor
//cout << "Current value of motorInt: " << currentState.motorInt << endl;
gpioServo(GPIO_MOTOR_CONTROL, currentState.motorInt);
}
//update servo
gpioServo(GPIO_STEERING_CONTROL, currentState.servoInt);
//toggle turn signal
//right
pthread_mutex_lock(&turnSignal_mutex);
if (parsedJson.get<bool>("R1")) {
if (currentState.rightTurn == true) {
currentState.rightTurn = false;
currentState.leftTurn = false;
//cout << "Right turn signal off." << endl;
} else {
currentState.rightTurn = true;
currentState.leftTurn = false;
//cout << "Right turn signal on." << endl;
}
}
//left
if (parsedJson.get<bool>("L1")) {
if (currentState.leftTurn == true) {
currentState.leftTurn = false;
currentState.rightTurn = false;
//cout << "Left turn signal off." << endl;
} else {
currentState.leftTurn = true;
currentState.rightTurn = false;
//cout << "Left turn signal on." << endl;
}
}
//check for peak
if (currentState.leftTurn || currentState.rightTurn) {
if (currentState.turnPeaked) {
if (currentState.servoInt == SERVO_CENTER) {
currentState.leftTurn = false;
currentState.rightTurn = false;
currentState.turnPeaked = false;
//cout << "Turn complete" << endl;
}
} else {
if (currentState.leftTurn) {
if (currentState.servoInt
< (SERVO_CENTER - TURN_THRESHOLD)) {
currentState.turnPeaked = true;
}
} else if (currentState.rightTurn) {
if (currentState.servoInt
> (SERVO_CENTER + TURN_THRESHOLD)) {
currentState.turnPeaked = true;
}
}
}
}
pthread_mutex_unlock(&turnSignal_mutex);
//toggle cruise mode
if (parsedJson.get<bool>("left")) {
if (currentState.cruise == true) {
currentState.cruise = false;
cout << "Cruise mode off" << endl;
} else {
currentState.cruise = true;
cout << "Cruise mode on" << endl;
}
}
if (parsedJson.get<bool>("up")) {
currentState.motorInt = currentState.motorInt + 10;
gpioServo(GPIO_MOTOR_CONTROL, currentState.motorInt);
//cout << "Current value of motorInt: " << currentState.motorInt << endl;
} else if (parsedJson.get<bool>("down")) {
currentState.motorInt = currentState.motorInt - 10;
gpioServo(GPIO_MOTOR_CONTROL, currentState.motorInt);
//cout << "Current value of motorInt: " << currentState.motorInt << endl;
}
}
pthread_mutex_unlock(&autonomous_mutex);
ptree answerJson;
stringstream jsonWriteStream;
answerJson.put("left_blinker", currentState.leftTurn);
answerJson.put("right_blinker", currentState.rightTurn);
write_json(jsonWriteStream, answerJson);
//cout << "sending speedometer" << endl;
if (currentState.connected) {
websocket_server.send(currentState.client, jsonWriteStream.str(),
websocketpp::frame::opcode::text);
}
//keep track of message recieved times.
unsigned long currentTime = time(NULL);
pthread_mutex_lock(&watchDog_mutex);
//cout << "currentTime=" << currentTime << endl;
currentState.lastMessageTime = currentTime;
currentState.connected = true;
pthread_mutex_unlock(&watchDog_mutex);
} catch (std::exception const& e) {
cerr << "<ERROR>" << endl;
cerr << e.what() << endl;
cerr << msg->get_payload() << endl;
cerr << "</ERROR>" << endl;
}
}
void on_open(connection_hdl hdl) {
holdThreads[hdl]=0;//just an example. Creates a new item for every connection. Gets deleted on close
m_server.send(hdl,"message", websocketpp::frame::opcode::text);
}
