Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Diff: Protocol/receiver.cpp
- Revision:
- 15:69d9e85382de
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Protocol/receiver.cpp Sun May 01 23:01:58 2016 +0000 @@ -0,0 +1,138 @@ +/* +Copyright 2016 Erik Perillo <erik.perillo@gmail.com> + +This file is part of piranha-ptc. + +This is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this. If not, see <http://www.gnu.org/licenses/>. +*/ + + +#include "receiver.h" +#include "EthernetInterface.h" + +float Receiver::un_scale(uint16_t value, float min, float max) +{ + return ((float)value)/((1 << 16) - 1)*(max - min) + min; +} + +uint8_t Receiver::get_header() +{ + return this->message[MSG_HEADER_IDX]; +} + +uint16_t Receiver::get_raw_val(int pos) +{ + uint16_t value = 0; + + value |= this->message[MSG_VALS_START_IDX + 2*pos]; + value |= this->message[MSG_VALS_START_IDX + 2*pos + 1] << 8; + + return value; +} + +float Receiver::get_val(float min, float max, int pos) +{ + uint16_t raw_val; + + raw_val = this->get_raw_val(pos); + return this->un_scale(raw_val, min, max); +} + +void Receiver::get_vals(float min, float max, float* vals, int size) +{ + uint16_t raw_val; + + for(int i=0; i<size; i++) + { + raw_val = this->get_raw_val(i); + vals[i] = this->un_scale(raw_val, min, max); + } +} + +bool Receiver::receive() +{ + return this->sock.receiveFrom(this->sender_addr, this->message, + sizeof(this->message)) > 0; +} + +Receiver::Receiver() +{ + ; +} + +Receiver::Receiver(Endpoint sender_addr, const UDPSocket& sock): + sock(sock), sender_addr(sender_addr) +{ + ; +} + +Receiver::Receiver(Endpoint sender_addr, int sock_port, int timeout): + sender_addr(sender_addr) +{ + this->sock.bind(sock_port); + this->sock.set_blocking(timeout < 0, timeout); +} + +void Receiver::set_sender_addr(const Endpoint& sender_addr) +{ + this->sender_addr = sender_addr; +} + +void Receiver::set_socket(const UDPSocket& sock) +{ + this->sock = sock; +} + +void Receiver::set_socket(int port, int timeout) +{ + this->sock.bind(port); + this->sock.set_blocking(timeout < 0, timeout); +} + +Endpoint Receiver::get_sender_addr() +{ + return this->sender_addr; +} + +UDPSocket Receiver::get_socket() +{ + return this->sock; +} + +uint8_t Receiver::get_msg() +{ + return this->message[MSG_HEADER_IDX]; +} + +float Receiver::get_ang_ref() +{ + return this->get_val(ANG_REF_MIN, ANG_REF_MAX); +} + +float Receiver::get_gnd_vel() +{ + return this->get_val(GND_VEL_MIN, GND_VEL_MAX); +} + +void Receiver::get_jog_vel(float* period, float* ratio) +{ + *period = this->get_val(JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX); + *ratio = this->get_val(JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX, 1); +} + +void Receiver::get_pid_params(float* params) +{ + this->get_vals(PID_PARAMS_MIN, PID_PARAMS_MAX, params, 4); +} +