Revision 20:7138ab2f93f7, committed 2016-07-16

Comitter:

drelliak

Date:

Sat Jul 16 19:17:08 2016 +0000

Parent:

19:c709c5a9fb08

Child:

21:8a98c6450e00

Commit message:

Winter Challenge 2016 Trekking controller

Changed in this revision

--- a/EthernetInterface.lib	Sun May 15 19:14:06 2016 +0000
+++ b/EthernetInterface.lib	Sat Jul 16 19:17:08 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/TrekkingPhoenix/code/EthernetInterface/#403af2883ca6
+http://developer.mbed.org/users/mbed_official/code/EthernetInterface/#4d7bff17a592

--- a/Motor/Motor.cpp	Sun May 15 19:14:06 2016 +0000
+++ b/Motor/Motor.cpp	Sat Jul 16 19:17:08 2016 +0000
@@ -3,12 +3,9 @@
 #include "Motor.h"
 
 #define PI 3.141592653589793238462
-#define Ts 0.02                         // Seconds
-#define PWM_PERIOD 13.5 // ms
-#define BRAKE_CONSTANT 40
-#define BRAKE_WAIT 2
-#define END_THRESH 4
-#define START_THRESH 10
+#define PWM_PERIOD 13.5                 // ms
+#define BRAKE_CONSTANT 100              // Brake force(max = 100)
+#define BRAKE_WAIT 1.662                // seconds
 #define MINIMUM_VELOCITY 15
 
 void Motor(){
@@ -39,11 +36,11 @@
     alternate_motor = !alternate_motor;
 }
 
-void Motor::brakeMotor(void){
+void Motor::brakeMotor(float brake_intensity, float brake_wait){
     stopJogging();
     if(velocity >= 0){
-        setMotorPWM(-BRAKE_CONSTANT, motor);
-        wait(BRAKE_WAIT);
+        setMotorPWM(-brake_intensity, motor);
+        wait(brake_wait);
         velocity = 0;
         setMotorPWM(velocity,motor);
     }
@@ -87,4 +84,7 @@
 void Motor::setVelocity(int new_velocity){
  setMotorPWM(new_velocity,motor);
  velocity = new_velocity;
-}
\ No newline at end of file
+}
+float Motor::getVelocity(){
+    return velocity;
+    }
\ No newline at end of file

--- a/Motor/Motor.h	Sun May 15 19:14:06 2016 +0000
+++ b/Motor/Motor.h	Sat Jul 16 19:17:08 2016 +0000
@@ -18,9 +18,10 @@
     
     void startJogging(float jog_dc, float jog_p);
     void stopJogging(void);
-    void brakeMotor(void);
+    void brakeMotor(float brake_intensity, float brake_wait);
     void reverseMotor(int speed);
     void setVelocity(int new_velocity);
+    float getVelocity();
     void setSmoothVelocity(int new_velocity);
     Motor(): motor(PTD1){}
     

--- a/Protocol/protocol.h	Sun May 15 19:14:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,121 +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
-//@}
-
-//@{
-///Angle reference from camera range (in radians).
-#define CAM_ANG_REF_MIN -PI
-#define CAM_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
-//@}
-
-//@{
-///Jogging speed period (in seconds).
-#define MAG_CALIB_MIN -1000.0
-#define MAG_CALIB_MAX 1000.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 angle reference from camera.
-	CAM_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
-

--- a/Protocol/receiver.cpp	Sun May 15 19:14:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,162 +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)
-{
-	int n = 32;
-	unsigned res;
-	socklen_t * val;
-	*val = sizeof(res);
-	this->sock.bind(port);
-	this->sock.set_blocking(timeout < 0, timeout);
-
-	this->sock.get_option(SOL_SOCKET, SO_RCVBUF, (unsigned*)&res, val);
-	printf("before setsock: %u\r\n", res);
-
-	printf("setsock: %d\r\n",
-		this->sock.set_option(SOL_SOCKET, SO_RCVBUF, &n, sizeof(n)));
-
-	this->sock.get_option(SOL_SOCKET, SO_RCVBUF, (unsigned*)&res, val);
-	printf("atfer setsock: %u\r\n",
-		res);
-}
-
-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_cam_ang_ref()
-{
-	return this->get_val(CAM_ANG_REF_MIN, CAM_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);
-}
-
-void Receiver::get_mag_calib(float* params)
-{
-	this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
-}
-

--- a/Protocol/receiver.h	Sun May 15 19:14:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,75 +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"
-
-#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_ang_ref();
-	float get_cam_ang_ref();
-	float get_gnd_vel();
-	void get_jog_vel(float* period, float* ratio);
-	void get_pid_params(float* params);
-	void get_mag_calib(float* params);
-};
-
-#endif
-

--- a/SensorsLibrary/FXAS21002.h	Sun May 15 19:14:06 2016 +0000
+++ b/SensorsLibrary/FXAS21002.h	Sat Jul 16 19:17:08 2016 +0000
@@ -27,7 +27,7 @@
 #define FXAS21002_CTRL_REG0 0x0D
 #define FXAS21002_CTRL_REG1 0x13
 #define FXAS21002_WHO_AM_I_VALUE 0xD1
-#define GYRO_OFFSET 0.03
+#define GYRO_OFFSET 0.09
 /*  Gyroscope mechanical modes
 *   Mode    Full-scale range [Deg/s]    Sensitivity [(mDeg/s)/LSB]
 *   1       +- 2000                     62.5

--- a/SensorsLibrary/FXOS8700.lib	Sun May 15 19:14:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/AswinSivakumar/code/FXOS8700/#98ea52282575

--- a/SensorsLibrary/FXOS8700CQ.lib	Sun May 15 19:14:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/trm/code/FXOS8700CQ/#e2fe752b881e

--- a/main.cpp	Sun May 15 19:14:06 2016 +0000
+++ b/main.cpp	Sat Jul 16 19:17:08 2016 +0000
@@ -4,22 +4,23 @@
 #include "CarPWM.h"
 #include "receiver.h"
 #include "Motor.h"
+#include "rtos.h"
 
-#define PI 3.141592653589793238462
-#define Ts 0.02                         // Seconds
+#define PI 3.141593
+#define Ts 0.02                         // Controller period(Seconds)
 #define PWM_PERIOD 13.5                 // ms
 #define INITIAL_P 0.452531214933414
 #define INITIAL_I 5.45748932024049
 #define INITIAL_D 0.000233453623255507
 #define INITIAL_N 51.0605584484153
-#define BRAKE_CONSTANT 40
-#define BRAKE_WAIT 0.3
-#define END_THRESH 4
-#define START_THRESH 10
+#define END_THRESH 4                    //For magnetometer calibration
+#define START_THRESH 10                 //For magnetometer calibration
 #define MINIMUM_VELOCITY 15
-#define GYRO_PERIOD 5000                //us
-#define LED_ON 0
-#define LED_OFF 1
+#define MINIMUM_CURVE_VELOCITY 19
+#define ERROR_THRESH    PI/5
+#define GYRO_PERIOD 15000               //us
+#define RGB_LED_ON 0                    //active Low
+#define RGB_LED_OFF 1                   //active Low
 
 #define MIN -1.5
 #define MAX 1.5
@@ -34,59 +35,19 @@
     AQUA,
     WHITE};
 
-void turn_leds_off(DigitalOut& red, DigitalOut& green, DigitalOut& blue)
-{
-    red = LED_OFF;
-    green = LED_OFF;
-    blue = LED_OFF;
-}
 
-void set_leds_color(int color, DigitalOut& red, DigitalOut& green, DigitalOut& blue)
-{
-    turn_leds_off(red, green, blue);
-
-    switch(color)
-    {
-        case RED:
-            red = LED_ON;               
-            break;
-        case GREEN:
-            green = LED_ON;
-            break;
-        case BLUE:
-            blue = LED_ON;
-            break;
-        case PURPLE:
-            red = LED_ON;
-            blue = LED_ON;
-            break;
-        case YELLOW:
-            red = LED_ON;
-            green = LED_ON;
-            break;
-        case AQUA:
-            blue = LED_ON;
-            green = LED_ON;
-            break;
-        case WHITE:
-            red = LED_ON;
-            green = LED_ON;
-            blue = LED_ON;
-            break;
-        default:
-            break;
-    }
-}
-
-Serial ser(USBTX, USBRX);    // Initialize Serial port
+//Control Objetcs
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
 Motor motor;
 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
 FXAS21002 gyro(PTE25,PTE24);
+//Leds Objects
 DigitalOut red_led(LED_RED);
 DigitalOut green_led(LED_GREEN);
 DigitalOut blue_led(LED_BLUE);
+DigitalOut main_led(PTD2);
+//Protocol Objects
 Receiver rcv;
 EthernetInterface eth;
 
@@ -98,18 +59,29 @@
 void control();
 Ticker controller_ticker;
 
-// Magnetometer variables and functions
+// Motor and servo variables
+float saved_velocity = 0;
+bool brake = false;
+
+// Magnetometer/Gyro variables and functions
 float max_x=0, max_y=0, min_x=0, min_y=0,x,y;
 MotionSensorDataUnits mag_data;
 MotionSensorDataCounts mag_raw;
 float processMagAngle();
+float gyro_reference = 0;
 void magCal();
 
 // Protocol
 void readProtocol();
+int contp = 0; // for debug only
+
+// NXP RGB_LEDs control
+void set_leds_color(int color);
+void turn_leds_off();
 
 int main(){
     // Initializing sensors:
+    main_led = 1;
     acc.enable();
     gyro.gyro_config(MODE_1);
     initializeController();
@@ -117,19 +89,22 @@
     // Set initial control configurations
     motor.setVelocity(0);
     
-    // Protocol parameters
+    // Protocol initialization
+    
     printf("Initializing Ethernet!\r\n");
-    set_leds_color(RED, red_led, green_led, blue_led);
+    set_leds_color(RED);
     eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
     eth.connect();
     printf("Protocol initialized! \r\n");
-    set_leds_color(BLUE, red_led, green_led, blue_led);
+    set_leds_color(BLUE);
     rcv.set_socket();
     gyro.start_measure(GYRO_PERIOD);
+    main_led = 0;
     controller_ticker.attach(&control,Ts);
     //main loop
     while(1){
         readProtocol();
+      //  printf("%f \r\n",gyro.get_angle());
     }
 }
 void control(){
@@ -139,77 +114,109 @@
     if(!rcv.receive())
         return;
     char msg = rcv.get_msg();
-    printf("Message received!");
+    //printf("Message received!");
+    //contp++;
+    //printf(" %d \r\n",contp);
     switch(msg)
     {
         case NONE:
-            //ser.printf("sending red signal to led\r\n");  
-            set_leds_color(BLACK,red_led,green_led,blue_led);          
-            printf("NONE\r\n");
-            wait(1);
             break;          
         case BRAKE:
-            //ser.printf("sending green signal to led\r\n");
-            set_leds_color(YELLOW,red_led,green_led,blue_led);
-            motor.stopJogging();
-            printf("BRAKE\r\n");
-            motor.brakeMotor();
+            //printf("BRAKE ");
+            float intensity, b_wait;
+            rcv.get_brake(&intensity,&b_wait);
+            if(!brake){
+                set_leds_color(YELLOW);
+                motor.stopJogging();
+               // printf("BRAKE\r\n");
+                setServoPWM(0,servo);
+                //reference = 0;
+                controller_ticker.detach();
+                motor.brakeMotor(intensity,b_wait);
+                controller_ticker.attach(&control,Ts);
+                saved_velocity = 0;
+                brake = true;
+                }
+            break;
+        case GYRO_ZERO:
+            gyro.stop_measure();
+            wait(0.05);
+            gyro.start_measure(GYRO_PERIOD);
+            break;
+        case ANG_SET:
+            set_leds_color(PURPLE);
+            //printf("ANG_SET\r\n");
+            gyro_reference = gyro.get_angle();
+            initializeController();
             break;
         case ANG_RST:
-            //ser.printf("sending blue signal to led\r\n");
-            set_leds_color(PURPLE,red_led,green_led,blue_led);
-            printf("ANG_RST\r\n");
-            gyro.stop_measure();
-            printf("Stopped gyro\r\n");
-            gyro.start_measure(GYRO_PERIOD);
-            printf("Starting Gyro\r\n");
+            //printf("ANG_RST\r\n");
+            gyro_reference = 0;
+            set_leds_color(PURPLE);
             initializeController();
             break;
-        case ANG_REF:
-            set_leds_color(GREEN,red_led,green_led,blue_led);
-            reference = rcv.get_ang_ref() - processMagAngle();
-            printf("New reference: %f \n\r",reference*180/PI);
+        case MAG_ANG_REF:
+            set_leds_color(BLUE);
+            reference = rcv.get_mag_ang_ref() - processMagAngle();
+            //printf("New reference: %f \n\r",reference*180/PI);
             if(reference > PI)
                 reference = reference - 2*PI;
             else if(reference < -PI)
                 reference = reference + 2*PI;  
             break;
-        case CAM_ANG_REF:
-            set_leds_color(RED,red_led,green_led,blue_led);
-            reference = rcv.get_cam_ang_ref();
-            printf("New reference: %f \n\r",reference*180/PI); 
+        case ABS_ANG_REF:
+            set_leds_color(GREEN);
+            reference = rcv.get_abs_ang_ref();
+            //printf("New reference: %f \n\r",reference*180/PI);
+            if(reference > PI)
+                reference = reference - 2*PI;
+            else if(reference < -PI)
+                reference = reference + 2*PI;  
+            break;
+        case REL_ANG_REF:
+            set_leds_color(RED);
+            reference = rcv.get_rel_ang_ref() + gyro.get_angle()*PI/180;
+            //printf("New reference: %f \n\r",reference*180/PI); 
+            if(reference > PI)
+                reference = reference - 2*PI;
+            else if(reference < -PI)
+                reference = reference + 2*PI;
             break;
         case GND_VEL:
-            set_leds_color(AQUA,red_led,green_led,blue_led);
-            float vel = rcv.get_gnd_vel();
-            motor.setVelocity(vel);      
-            printf("GND_VEL = %f\r\n", vel);
+            set_leds_color(AQUA);
+            saved_velocity = rcv.get_gnd_vel();
+            //printf("GND_VEL");
+            if(saved_velocity > 0){
+                motor.setVelocity(saved_velocity);      
+                if(abs(saved_velocity) > MINIMUM_VELOCITY)
+                    brake = false;
+                //printf("GND_VEL = %f\r\n", saved_velocity);
+            }
             break;
         case JOG_VEL:
-            set_leds_color(WHITE,red_led,green_led,blue_led);
+            set_leds_color(WHITE);
             float p, r;
             rcv.get_jog_vel(&p,&r);
-            printf("Joggin with period %f and duty cycle %f\r\n",p,r);
+            //printf("Joggin with period %f and duty cycle %f\r\n",p,r);
             if(p == 0 || r == 0)
                 motor.stopJogging();
             else
                 motor.startJogging(r,p);
             break;
         case PID_PARAMS:
-            set_leds_color(RED,red_led,green_led,blue_led);
-            float ar[4];
-            rcv.get_pid_params(ar);
-            P = ar[0];
-            I = ar[1];
-            D = ar[2];
-            N = ar[3];
-            printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", 
-                ar[0], ar[1], ar[2], ar[3]);
+            set_leds_color(WHITE);
+            float arr[4];
+            rcv.get_pid_params(arr);
+            P = arr[0];
+            I = arr[1];
+            D = arr[2];
+            N = arr[3];
+           // printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", 
+           //     arr[0], arr[1], arr[2], arr[3]);
 
-            wait(1);
             break;
         case MAG_CALIB:
-            set_leds_color(BLUE,red_led,green_led,blue_led);
+            set_leds_color(BLUE);
             printf("MAG_CALIB\r\n");
             float mag[4];
             rcv.get_mag_calib(mag);
@@ -217,10 +224,17 @@
             max_y=mag[3];
             min_x=mag[0];
             min_y=mag[2];
-            printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
+            //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
+            break;
+        case LED_ON:
+            set_leds_color(BLACK);
+            main_led = 1;
+            break;
+        case LED_OFF:
+            set_leds_color(BLACK);
+            main_led = 0;
             break;
         default:
-            printf("nothing understood\r\n");
             //ser.printf("unknown command!\r\n");
     }
 }
@@ -240,7 +254,7 @@
 /* PID controller for angular position */
 void controlAnglePID(float P, float I, float D, float N){ 
     /* Getting error */
-    float feedback = gyro.get_angle();
+    float feedback = gyro.get_angle() - gyro_reference;
     e[1] = e[0];
     e[0] = reference - (feedback*PI/180);
     if(e[0] > PI)
@@ -264,6 +278,13 @@
     /** Controller **/ 
     u = up[0] + ud[0] + ui[0];
     setServoPWM(u*100/(PI/8), servo);
+    if(abs(e[0]) >= ERROR_THRESH && motor.getVelocity() > MINIMUM_CURVE_VELOCITY){
+        saved_velocity = motor.getVelocity();
+        motor.setVelocity(MINIMUM_CURVE_VELOCITY);
+        }
+    else if(abs(e[0]) < ERROR_THRESH && motor.getVelocity() != saved_velocity){
+        motor.setVelocity(saved_velocity);
+        }
 }
 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
 /* Function to normalize the magnetometer reading */
@@ -295,7 +316,7 @@
 
 /* Function to transform the magnetometer reading in angle(rad/s).*/
 float processMagAngle(){
-    printf("starting ProcessMagAngle()\n\r");
+   // printf("starting ProcessMagAngle()\n\r");
     float mag_lpf = 0;
     Timer t1;
     for(int i = 0; i<20; i++){
@@ -312,6 +333,49 @@
         wait(0.015);
         }
    // wait(20*0.01);
-    printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
+   // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
     return mag_lpf/20;
+}
+void turn_leds_off()
+{
+    red_led = RGB_LED_OFF;
+    green_led = RGB_LED_OFF;
+    blue_led = RGB_LED_OFF;
+}
+
+void set_leds_color(int color)
+{
+    turn_leds_off();
+
+    switch(color)
+    {
+        case RED:
+            red_led = RGB_LED_ON;               
+            break;
+        case GREEN:
+            green_led = RGB_LED_ON;
+            break;
+        case BLUE:
+            blue_led = RGB_LED_ON;
+            break;
+        case PURPLE:
+            red_led = RGB_LED_ON;
+            blue_led = RGB_LED_ON;
+            break;
+        case YELLOW:
+            red_led = RGB_LED_ON;
+            green_led = RGB_LED_ON;
+            break;
+        case AQUA:
+            blue_led = RGB_LED_ON;
+            green_led = RGB_LED_ON;
+            break;
+        case WHITE:
+            red_led = RGB_LED_ON;
+            green_led = RGB_LED_ON;
+            blue_led = RGB_LED_ON;
+            break;
+        default:
+            break;
+    }
 }
\ No newline at end of file

--- a/mbed-rtos.lib	Sun May 15 19:14:06 2016 +0000
+++ b/mbed-rtos.lib	Sat Jul 16 19:17:08 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed-rtos/#7a567bf048bf
+http://mbed.org/users/mbed_official/code/mbed-rtos/#162b12aea5f2