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;