Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Revision 16:a9e0eb97557f, committed 2016-05-02
- Comitter:
- drelliak
- Date:
- Mon May 02 00:57:58 2016 +0000
- Parent:
- 15:69d9e85382de
- Child:
- 17:34fa73413afb
- Commit message:
- Added the magnetometer command
Changed in this revision
--- a/Protocol/protocol.h Sun May 01 23:01:58 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,103 +0,0 @@ -/** -@file protocol.h -@brief Protocol definitions. -*/ - -/* -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/>. -*/ - - -#ifndef __PIRANHA_PROTOCOL_H__ -#define __PIRANHA_PROTOCOL_H__ - -//@{ -///PID parameters range. -#define PID_PARAMS_MIN -100.0 -#define PID_PARAMS_MAX 100.0 -//@} - -//@{ -///Ground velocity range. -#define GND_VEL_MIN -100.0 -#define GND_VEL_MAX 100.0 -//@} - -//@{ -///Angle reference range (in radians). -#define PI 3.141593 -#define ANG_REF_MIN -PI -#define ANG_REF_MAX PI -//@} - -//@{ -///Jogging speed period (in seconds). -#define JOG_VEL_PERIOD_MIN 0.0 -#define JOG_VEL_PERIOD_MAX 300.0 -//@} - -//@{ -///Jogging speed ratio. -#define JOG_VEL_RATIO_MIN 0.0 -#define JOG_VEL_RATIO_MAX 1.0 -//@} - -///Messages to send via protocol. -enum -{ - ///Do nothing. - NONE, - - ///Brake the robot. - BRAKE, - - ///Reset angle measurement. - ANG_RST, - - ///Set new angle reference. - ANG_REF, - - ///Set new ground velocity for robot. - GND_VEL, - - ///Set new jogging speed for robot. - JOG_VEL, - - ///Send PID control parameters. - PID_PARAMS -}; - -#define MSG_HEADER_SIZE 1 -#define MSG_VAL_SIZE 2 -#define MSG_MAX_NUM_VALS 4 -#define MSG_BUF_LEN (MSG_HEADER_SIZE + MSG_VAL_SIZE*MSG_MAX_NUM_VALS) -#define MSG_HEADER_IDX 0 -#define MSG_VALS_START_IDX (MSG_HEADER_IDX + 1) - -#define SENDER_PORT 7532 -#define SENDER_IFACE_ADDR "192.168.7.1" -#define SENDER_NETMASK_ADDR "255.255.255.0" -#define SENDER_GATEWAY_ADDR "0.0.0.0" - -#define RECEIVER_PORT 7533 -#define RECEIVER_IFACE_ADDR "192.168.7.2" -#define RECEIVER_NETMASK_ADDR "255.255.255.0" -#define RECEIVER_GATEWAY_ADDR "0.0.0.0" - -#endif -
--- a/Protocol/receiver.cpp Sun May 01 23:01:58 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,138 +0,0 @@ -/* -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); -} -
--- a/Protocol/receiver.h Sun May 01 23:01:58 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,67 +0,0 @@ -/** -@file receiver.h -@brief Receiver side functions declarations. -*/ - -/* -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/>. -*/ - - -#ifndef __PIRANHA_RCV_PROTOCOL_H__ -#define __PIRANHA_RCV_PROTOCOL_H__ - -#include "protocol.h" -#include "mbed.h" -#include "EthernetInterface.h" - -class Receiver -{ - //protected: - public: - UDPSocket sock; - char message[MSG_BUF_LEN]; - Endpoint sender_addr; - - float un_scale(uint16_t value, float min, float max); - uint8_t get_header(); - uint16_t get_raw_val(int pos=0); - float get_val(float min, float max, int pos=0); - void get_vals(float min, float max, float* vals, int size); - - public: - Receiver(); - Receiver(Endpoint sender_addr, const UDPSocket& sock); - Receiver(Endpoint sender_addr, int sock_port=RECEIVER_PORT, int timeout=1); - - void set_sender_addr(const Endpoint& sender_addr); - void set_socket(const UDPSocket& sock); - void set_socket(int port=RECEIVER_PORT, int timeout=1); - Endpoint get_sender_addr(); - UDPSocket get_socket(); - - bool receive(); - uint8_t get_msg(); - float get_ang_ref(); - float get_gnd_vel(); - void get_jog_vel(float* period, float* ratio); - void get_pid_params(float* params); -}; - -#endif -
--- a/main.cpp Sun May 01 23:01:58 2016 +0000 +++ b/main.cpp Mon May 02 00:57:58 2016 +0000 @@ -111,7 +111,6 @@ int main(){ // Initializing sensors: acc.enable(); - //magCal(); gyro.gyro_config(MODE_1); initializeController(); @@ -222,6 +221,14 @@ blue_led = LED_OFF; green_led = LED_OFF; break; + case MAG_CALIB: + float mag[4]; + rcv.get_mag_calib(mag); + max_x=mag[1]; + max_y=mag[3]; + min_x=mag[0]; + min_y=mag[2]; + break; default: blue_led = LED_ON; green_led = LED_ON;