Quentin Mettraux / Mbed OS MAVErIC_TEST

Working Maveric

RegulationScript.cpp@10:36a2131f636c, 2017-08-22 (annotated)

Committer:
mettrque
Date:
Tue Aug 22 10:49:39 2017 +0000
Revision:
10:36a2131f636c
Parent:
6:ef95300898b2 
last military version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mettrque 0:bdca5e4773dd 1 /*
mettrque 0:bdca5e4773dd 2 * RegulationScript.cpp
mettrque 0:bdca5e4773dd 3 * Copyright (c) 2017, ZHAW
mettrque 0:bdca5e4773dd 4 * All rights reserved.
mettrque 0:bdca5e4773dd 5 *
mettrque 0:bdca5e4773dd 6 * Created on: 04.04.2017
mettrque 0:bdca5e4773dd 7 * Author: Quentin Mettraux
mettrque 0:bdca5e4773dd 8 */
mettrque 0:bdca5e4773dd 9
mettrque 0:bdca5e4773dd 10 #include <stdint.h>
mettrque 0:bdca5e4773dd 11 #include "Controller.h"
mettrque 6:ef95300898b2 12 #include "StateMachine.h"
mettrque 0:bdca5e4773dd 13 #include "RegulationScript.h"
mettrque 0:bdca5e4773dd 14
mettrque 0:bdca5e4773dd 15 using namespace std;
mettrque 0:bdca5e4773dd 16
mettrque 5:ebe6a0242150 17 template <typename T> int sgn(T val)
mettrque 5:ebe6a0242150 18 {
mettrque 5:ebe6a0242150 19 return (T(0) < val) - (val < T(0));
mettrque 5:ebe6a0242150 20 }
mettrque 5:ebe6a0242150 21
mettrque 0:bdca5e4773dd 22 inline string bool2String(bool b)
mettrque 0:bdca5e4773dd 23 {
mettrque 0:bdca5e4773dd 24
mettrque 0:bdca5e4773dd 25 return string(b ? "true" : "false");
mettrque 0:bdca5e4773dd 26 }
mettrque 0:bdca5e4773dd 27
mettrque 0:bdca5e4773dd 28 inline string int2String(int32_t i)
mettrque 0:bdca5e4773dd 29 {
mettrque 0:bdca5e4773dd 30
mettrque 0:bdca5e4773dd 31 char buffer[32];
mettrque 0:bdca5e4773dd 32 sprintf(buffer, "%d", i);
mettrque 0:bdca5e4773dd 33
mettrque 0:bdca5e4773dd 34 return string(buffer);
mettrque 0:bdca5e4773dd 35 }
mettrque 0:bdca5e4773dd 36
mettrque 0:bdca5e4773dd 37 inline string float2String(float f,uint16_t d)
mettrque 0:bdca5e4773dd 38 {
mettrque 0:bdca5e4773dd 39
mettrque 0:bdca5e4773dd 40 char buffer[32];
mettrque 0:bdca5e4773dd 41 if (d <= 1) sprintf(buffer, "%.1f", f);
mettrque 0:bdca5e4773dd 42 else if (d == 2) sprintf(buffer, "%.2f", f);
mettrque 0:bdca5e4773dd 43 else if (d == 3) sprintf(buffer, "%.3f", f);
mettrque 0:bdca5e4773dd 44 else if (d == 4) sprintf(buffer, "%.4f", f);
mettrque 0:bdca5e4773dd 45 else if (d == 5) sprintf(buffer, "%.5f", f);
mettrque 0:bdca5e4773dd 46 else sprintf(buffer, "%.6f", f);
mettrque 0:bdca5e4773dd 47
mettrque 0:bdca5e4773dd 48 return string(buffer);
mettrque 0:bdca5e4773dd 49 }
mettrque 0:bdca5e4773dd 50
mettrque 0:bdca5e4773dd 51 /**
mettrque 0:bdca5e4773dd 52 * Creates a http script with a reference to a controller object.
mettrque 0:bdca5e4773dd 53 * @param controller a reference to the robot controller object.
mettrque 10:36a2131f636c 54 * @param stateMachine a reference to the state machine object.
mettrque 0:bdca5e4773dd 55 */
mettrque 6:ef95300898b2 56 RegulationScript::RegulationScript(Controller& controller, StateMachine& stateMachine) : HTTPScript(), controller(controller), stateMachine(stateMachine)
mettrque 5:ebe6a0242150 57 {
mettrque 5:ebe6a0242150 58 timer.start();
mettrque 5:ebe6a0242150 59 }
mettrque 0:bdca5e4773dd 60
mettrque 0:bdca5e4773dd 61 RegulationScript::~RegulationScript() {}
mettrque 0:bdca5e4773dd 62
mettrque 0:bdca5e4773dd 63 /**
mettrque 0:bdca5e4773dd 64 * This method contains the logic to be executed when this http script is called.
mettrque 0:bdca5e4773dd 65 * It creates a response with an xml encoded process value from the controller.
mettrque 0:bdca5e4773dd 66 */
mettrque 0:bdca5e4773dd 67 string RegulationScript::call(vector<string> names, vector<string> values)
mettrque 0:bdca5e4773dd 68 {
mettrque 6:ef95300898b2 69 // reset watchdog timer
mettrque 0:bdca5e4773dd 70
mettrque 6:ef95300898b2 71 stateMachine.resetWatchdogTimer();
mettrque 5:ebe6a0242150 72
mettrque 0:bdca5e4773dd 73 // parse and process given arguments
mettrque 5:ebe6a0242150 74
mettrque 0:bdca5e4773dd 75 for (uint16_t i = 0; i < min(names.size(), values.size()); i++) {
mettrque 0:bdca5e4773dd 76 if (names[i].compare("kp1") == 0) {
mettrque 5:ebe6a0242150 77 controller.setTuningsSpeedController(1,strtof(values[i].c_str(), NULL));
mettrque 0:bdca5e4773dd 78 } else if (names[i].compare("ki1") == 0) {
mettrque 5:ebe6a0242150 79 controller.setTuningsSpeedController(2,strtof(values[i].c_str(), NULL));
mettrque 0:bdca5e4773dd 80 } else if (names[i].compare("kd1") == 0) {
mettrque 5:ebe6a0242150 81 controller.setTuningsSpeedController(3,strtof(values[i].c_str(), NULL));
mettrque 0:bdca5e4773dd 82 } else if (names[i].compare("kp2") == 0) {
mettrque 5:ebe6a0242150 83 controller.setTuningsSteeringController(1,strtof(values[i].c_str(), NULL));
mettrque 0:bdca5e4773dd 84 } else if (names[i].compare("ki2") == 0) {
mettrque 5:ebe6a0242150 85 controller.setTuningsSteeringController(2,strtof(values[i].c_str(), NULL));
mettrque 0:bdca5e4773dd 86 } else if (names[i].compare("kd2") == 0) {
mettrque 5:ebe6a0242150 87 controller.setTuningsSteeringController(3,strtof(values[i].c_str(), NULL));
mettrque 5:ebe6a0242150 88 } else if (names[i].compare("saveConfiguration") == 0){
mettrque 5:ebe6a0242150 89 printf("Save PID Parameter\r\n");
mettrque 0:bdca5e4773dd 90 controller.saveConfiguration();
mettrque 0:bdca5e4773dd 91 }
mettrque 0:bdca5e4773dd 92 }
mettrque 5:ebe6a0242150 93
mettrque 0:bdca5e4773dd 94 // create response
mettrque 5:ebe6a0242150 95
mettrque 0:bdca5e4773dd 96 string response;
mettrque 5:ebe6a0242150 97
mettrque 6:ef95300898b2 98 //Motion motorSpeed[4];
mettrque 6:ef95300898b2 99 //controller.getMotorSpeed(motorSpeed);
mettrque 5:ebe6a0242150 100
mettrque 0:bdca5e4773dd 101 response += " <controller>\r\n";
mettrque 5:ebe6a0242150 102 response += " <time><int>"+int2String(timer.read_ms())+"</int></time>\r\n";
mettrque 5:ebe6a0242150 103 response += " <kp1><float>"+float2String(controller.getTuningsSpeedController(1), 4)+"</float></kp1>\r\n";
mettrque 5:ebe6a0242150 104 response += " <ki1><float>"+float2String(controller.getTuningsSpeedController(2), 4)+"</float></ki1>\r\n";
mettrque 5:ebe6a0242150 105 response += " <kd1><float>"+float2String(controller.getTuningsSpeedController(3), 4)+"</float></kd1>\r\n";
mettrque 5:ebe6a0242150 106 response += " <kp2><float>"+float2String(controller.getTuningsSteeringController(1), 4)+"</float></kp2>\r\n";
mettrque 5:ebe6a0242150 107 response += " <ki2><float>"+float2String(controller.getTuningsSteeringController(2), 4)+"</float></ki2>\r\n";
mettrque 5:ebe6a0242150 108 response += " <kd2><float>"+float2String(controller.getTuningsSteeringController(3), 4)+"</float></kd2>\r\n";
mettrque 0:bdca5e4773dd 109 response += " </controller>\r\n";
mettrque 5:ebe6a0242150 110
mettrque 0:bdca5e4773dd 111 return response;
mettrque 5:ebe6a0242150 112 }
mettrque 0:bdca5e4773dd 113