Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
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