File content as of revision 10:36a2131f636c:

/*
 * ControllerScript.cpp
 * Copyright (c) 2017, ZHAW
 * All rights reserved.
 *
 *  Created on: 02.05.2017
 *      Author: Quentin Mettraux
 */

#include <stdint.h>
#include "Controller.h"
#include "StateMachine.h"
#include "ControllerScript.h"

using namespace std;

template <typename T> int sgn(T val)
{
    return (T(0) < val) - (val < T(0));
}

inline string bool2String(bool b)
{

    return string(b ? "true" : "false");
}

inline string int2String(int32_t i)
{

    char buffer[32];
    sprintf(buffer, "%d", i);

    return string(buffer);
}

inline string float2String(float f,uint16_t d)
{

    char buffer[32];
    if (d == 0) sprintf(buffer, "%.0f", f);
    else if (d == 1) sprintf(buffer, "%.1f", f);
    else if (d == 2) sprintf(buffer, "%.2f", f);
    else if (d == 3) sprintf(buffer, "%.3f", f);
    else if (d == 4) sprintf(buffer, "%.4f", f);
    else if (d == 5) sprintf(buffer, "%.5f", f);
    else sprintf(buffer, "%.6f", f);

    return string(buffer);
}


/**
 * Creates a http script with a reference to a controller object.
 * @param controller a reference to the robot controller object.
 * @param stateMachine a reference to the state machine object.
 */
ControllerScript::ControllerScript(Controller& controller, StateMachine& stateMachine) : HTTPScript(), controller(controller), stateMachine(stateMachine) {}

ControllerScript::~ControllerScript() {}

/**
 * This method contains the logic to be executed when this http script is called.
 * It creates a response with an xml encoded process value from the controller.
 */
string ControllerScript::call(vector<string> names, vector<string> values)
{

    // reset watchdog timer

    stateMachine.resetWatchdogTimer();

    // parse and process arguments

    for (int32_t i = 0; i < min(names.size(), values.size()); i++) {

        if (names[i].compare("state") == 0) {

            if (values[i].compare("off") == 0) {
                stateMachine.setEnable(0);
                stateMachine.setEnableMoving(0);
                stateMachine.setEnablePositioning(0);
            } else if (values[i].compare("on") == 0) {
                stateMachine.setEnable(1);
                stateMachine.setEnableMoving(0);
                stateMachine.setEnablePositioning(0);
            } else if (values[i].compare("moveManually") == 0)  {
                stateMachine.setEnable(1);
                stateMachine.setEnablePositioning(0);
                stateMachine.setEnableMoving(1);
            } else if (values[i].compare("moveToPose") == 0) {
                stateMachine.setEnable(1);
                stateMachine.setEnableMoving(0);
                stateMachine.setEnablePositioning(1);
            }
        } else if (names[i].compare("setVelocity") == 0 && stateMachine.getState() == StateMachine::STATE_ON) {
            for(int32_t i = 0; i < min(names.size(), values.size()); i++) {
                if(names[i].compare("translationVelo") == 0) {
                    controller.setTranslationalVelocity(strtof(values[i].c_str(), NULL));
                } else if(names[i].compare("desiredSteeringAngle") == 0) {
                    controller.setSteeringAngle(strtof(values[i].c_str(), NULL)*Controller::PI/180.0f);
                }

            }
        } else if (names[i].compare("setPosition") == 0 && stateMachine.getState() == StateMachine::STATE_OFF) {
            for(int32_t i = 0; i < min(names.size(), values.size()); i++) {
                if(names[i].compare("xCoord") == 0) {
                    controller.setPositionX(strtof(values[i].c_str(), NULL));
                } else if(names[i].compare("yCoord") == 0) {
                    controller.setPositionY(strtof(values[i].c_str(), NULL));
                } else if(names[i].compare("zCoord") == 0) {
                    controller.setPositionZ(strtof(values[i].c_str(), NULL));
                } else if(names[i].compare("alphaCoord") == 0) {
                    controller.setOrientation(strtof(values[i].c_str(), NULL)*Controller::PI/180.0f);
                }
            }
        } else if (names[i].compare("joystick") == 0 && stateMachine.getState() == StateMachine::STATE_MOVE_MANUALLY) {
            for(int32_t i = 0; i < min(names.size(), values.size()); i++) {
                if(names[i].compare("axisX") == 0) {
                    controller.setSteeringAngle(strtof(values[i].c_str(), NULL)*Controller::MAX_STEERING_ANGLE);
                } else if(names[i].compare("axisY") == 0) {
                    controller.setTranslationalVelocity(strtof(values[i].c_str(), NULL)*0.5f);
                }
            }
        }
    }
// create response

    string response;

    response += "  <controller>\r\n";
    response += "    <x><float>"+float2String(controller.getPositionX(),3)+"</float></x>\r\n";
    response += "    <y><float>"+float2String(controller.getPositionY(),3)+"</float></y>\r\n";
    response += "    <z><float>"+float2String(controller.getPositionZ(),3)+"</float></z>\r\n";
    response += "    <alpha><float>"+float2String(controller.getOrientation()*180.0f/Controller::PI,3)+"</float></alpha>\r\n";
    response += "    <steeringAngle><float>"+float2String(controller.getSteeringAngle()*180.0f/Controller::PI,3)+"</float></steeringAngle>\r\n";
    response += "    <translation><float>"+float2String(controller.getActualTranslationalVelocity(),3)+"</float></translation>\r\n";
    response += "    <rotation><float>"+float2String(controller.getActualRotationalVelocity(),3)+"</float></rotation>\r\n";
    response += "    <voltage24><float>"+float2String(stateMachine.getVoltage24V(),2)+"</float></voltage24>\r\n";
    response += "    <voltage36><float>"+float2String(stateMachine.getVoltage36V(),2)+"</float></voltage36>\r\n";
    response += "    <state><int>"+int2String(stateMachine.getState())+"</int></state>\r\n";
    response += "  </controller>\r\n";

    return response;
}