Revision 21:8a98c6450e00, committed 2016-07-16

Comitter:

drelliak

Date:

Sat Jul 16 19:17:28 2016 +0000

Parent:

20:7138ab2f93f7

Child:

22:b7cca3089dfe

Commit message:

Winter Challenge trekking controller

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/protocol.h	Sat Jul 16 19:17:28 2016 +0000
@@ -0,0 +1,154 @@
+/**
+@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 0.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 ABS_ANG_REF_MIN -PI
+#define ABS_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from robot range (in radians).
+#define REL_ANG_REF_MIN -PI
+#define REL_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from magnetometer range (in radians).
+#define MAG_ANG_REF_MIN -PI
+#define MAG_ANG_REF_MAX PI
+//@}
+
+//@{
+///Break intensity
+#define BRAKE_INTENSITY_MIN 0.0
+#define BRAKE_INTENSITY_MAX 100.0
+//@}
+
+//@{
+///Jogging speed ratio.
+#define BRAKE_PERIOD_MIN 0.0
+#define BRAKE_PERIOD_MAX 100.0
+//@}
+
+//@{
+///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
+//@}
+
+//@{
+///Magnetometer calibration values.
+#define MAG_CALIB_MIN -750.0
+#define MAG_CALIB_MAX 750.0
+//@}
+
+///Messages to send via protocol.
+enum
+{
+	///Do nothing.
+	NONE,
+
+	///Brake the robot.
+	BRAKE,
+
+	///Reset gyroscope.
+	GYRO_ZERO,
+
+	///Set zero axis to current angle measure.
+	ANG_SET,
+
+	///Reset angle zero axis.
+	ANG_RST,
+
+	///Turn on leds
+	LED_ON,
+
+	///Turn off leds
+	LED_OFF,
+
+	///Set new angle reference relative to zero axis.
+	ABS_ANG_REF,
+
+	///Set new angle reference relative to robot axis.
+	REL_ANG_REF,
+
+	///Set new angle reference relative to north using magnetometer.
+	MAG_ANG_REF,
+
+	///Set new ground velocity for robot.
+	GND_VEL,
+
+	///Set new jogging speed for robot.
+	JOG_VEL,
+
+	///Magnetometer calibration (min_x, max_x, min_y, max_y).
+	MAG_CALIB,
+
+	///Send PID control parameters (P, I, D, N).
+	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.2"
+#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.3"
+#define RECEIVER_NETMASK_ADDR "255.255.255.0"
+#define RECEIVER_GATEWAY_ADDR "0.0.0.0"
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/receiver.cpp	Sat Jul 16 19:17:28 2016 +0000
@@ -0,0 +1,159 @@
+/*
+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_abs_ang_ref()
+{
+	return this->get_val(ABS_ANG_REF_MIN, ABS_ANG_REF_MAX);
+}
+
+float Receiver::get_rel_ang_ref()
+{
+	return this->get_val(REL_ANG_REF_MIN, REL_ANG_REF_MAX);
+}
+
+float Receiver::get_mag_ang_ref()
+{
+	return this->get_val(MAG_ANG_REF_MIN, MAG_ANG_REF_MAX);
+}
+
+float Receiver::get_gnd_vel()
+{
+	return this->get_val(GND_VEL_MIN, GND_VEL_MAX);
+}
+
+void Receiver::get_brake(float* intensity, float* period)
+{
+	*intensity = this->get_val(BRAKE_INTENSITY_MIN, BRAKE_INTENSITY_MAX);
+	*period = this->get_val(BRAKE_PERIOD_MIN, BRAKE_PERIOD_MAX, 1);
+}
+
+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);
+}
+
+void Receiver::get_mag_calib(float* params)
+{
+	this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/receiver.h	Sat Jul 16 19:17:28 2016 +0000
@@ -0,0 +1,77 @@
+/**
+@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"
+
+#define TEST
+
+class Receiver
+{
+        
+	#ifdef TEST
+	public:
+	#else
+	protected:
+	#endif
+	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_abs_ang_ref();
+	float get_rel_ang_ref();
+	float get_mag_ang_ref();
+	float get_gnd_vel();
+	void get_brake(float* intensity, float* period);
+	void get_jog_vel(float* period, float* ratio);
+	void get_pid_params(float* params);
+	void get_mag_calib(float* params);
+};
+
+#endif
+