Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Revision 14:e8cd237c8639, committed 2016-05-01
- Comitter:
- drelliak
- Date:
- Sun May 01 23:01:27 2016 +0000
- Parent:
- 13:f7a7fe9b5c00
- Child:
- 15:69d9e85382de
- Commit message:
- Some minor fixes
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Sun May 01 23:01:27 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/EthernetInterface/#f81b90d9a441
--- a/Motor/Motor.cpp Sat Apr 30 21:28:27 2016 +0000 +++ b/Motor/Motor.cpp Sun May 01 23:01:27 2016 +0000 @@ -23,6 +23,7 @@ void Motor::stopJogging(void){ interruption.detach(); + setMotorPWM(velocity,motor); } void Motor::motorJogging(void) {
--- a/Protocol/protocol.h Sat Apr 30 21:28:27 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,55 +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/>. -*/ - - -#ifndef __PIRANHA_PROTOCOL_H__ -#define __PIRANHA_PROTOCOL_H__ - -#define PI 3.141593 - -//pid values range -#define PID_PARAMS_MIN -100.0 -#define PID_PARAMS_MAX 100.0 - -//ground speed range -#define GND_SPEED_MIN -100.0 -#define GND_SPEED_MAX 100.0 - -//angle reference range (in radians) -#define ANG_REF_MIN -PI -#define ANG_REF_MAX PI - -//messages to send via protocol -enum -{ - //do nothing - NONE, - //brake the robot - BRAKE, - //reset angle measure - ANG_RST, - //set new angle reference - ANG_REF, - //set new ground speed for robot - GND_SPEED, - //send pid control parameters - PID_PARAMS -}; - -#endif
--- a/Protocol/receiver.cpp Sat Apr 30 21:28:27 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,63 +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" - - -uint16_t read(Serial& serial) -{ - int i; - uint16_t value = 0; - char chr; - - for(i=0; i<2; i++) - while(true) - if(serial.readable()) - { - chr = serial.getc(); - value = value | chr << i*8; - break; - } - - return value; -} - -float un_scale(uint16_t value, float min, float max) -{ - return ((float)value)/((1 << 16) - 1)*(max - min) + min; -} - -float get_param(Serial& serial, float min, float max) -{ - uint16_t value; - - value = read(serial); - - return un_scale(value, min, max); -} - -void get_pid_params(Serial& serial, float* kp, float* ki, float* kd, float* n) -{ - *kp = get_pid_param(serial); - *ki = get_pid_param(serial); - *kd = get_pid_param(serial); - *n = get_pid_param(serial); -} -
--- a/Protocol/receiver.h Sat Apr 30 21:28:27 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,56 +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/>. -*/ - - -#ifndef __PIRANHA_RCV_PROTOCOL_H__ -#define __PIRANHA_RCV_PROTOCOL_H__ - -#include "protocol.h" -#include "mbed.h" - - -/** - Reads serial and converts a string of (one or two) bytes into an unsigned int. - Least-significant bytes must come first. - Assumes big-endian representation of numbers on architecture. -*/ -uint16_t read(Serial& serial); - -/** - Converts unsigned int into float following unit-normalization rules. -*/ -float un_scale(uint16_t value, float min, float max); - -/** - Applies un_scale to value read on serial. -*/ -float get_param(Serial& serial, float min, float max); - -/** - Gets all 4 pid parameters from serial stream. -*/ -void get_pid_params(Serial& serial, float* kp, float* ki, float* kd, float* n); - -//macros just to make life easier -#define get_pid_param(serial) get_param(serial, PID_PARAMS_MIN, PID_PARAMS_MAX) -#define get_gnd_speed(serial) get_param(serial, GND_SPEED_MIN, GND_SPEED_MAX) -#define get_ang_ref(serial) get_param(serial, ANG_REF_MIN, ANG_REF_MAX) - -#endif -
--- a/SensorsLibrary/FXAS21002.cpp Sat Apr 30 21:28:27 2016 +0000 +++ b/SensorsLibrary/FXAS21002.cpp Sun May 01 23:01:27 2016 +0000 @@ -31,7 +31,7 @@ gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2); d[0] = FXAS21002_CTRL_REG0; //Sets FSR and Sensitivity - d[1] = mode; + d[1] = mode + 0x80; gyroi2c.write(FXAS21002_I2C_ADDRESS, d, 2); d[0] = FXAS21002_CTRL_REG1; //Puts device in active mode @@ -42,7 +42,8 @@ void FXAS21002::stop_measure(void) { - interrupt.detach(); + interrupt.detach(); + angle = 0; } float FXAS21002::get_angle(void)
--- a/SensorsLibrary/FXAS21002.h Sat Apr 30 21:28:27 2016 +0000 +++ b/SensorsLibrary/FXAS21002.h Sun May 01 23:01:27 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.0093 +#define GYRO_OFFSET 0.03 /* Gyroscope mechanical modes * Mode Full-scale range [Deg/s] Sensitivity [(mDeg/s)/LSB] * 1 +- 2000 62.5
--- a/main.cpp Sat Apr 30 21:28:27 2016 +0000 +++ b/main.cpp Sun May 01 23:01:27 2016 +0000 @@ -17,69 +17,221 @@ #define END_THRESH 4 #define START_THRESH 10 #define MINIMUM_VELOCITY 15 -#define GYRO_PERIOD 1300 //us +#define GYRO_PERIOD 5000 //us +#define LED_ON 0 +#define LED_OFF 1 + +#define MIN -1.5 +#define MAX 1.5 + +enum{ + BLACK, + RED, + GREEN, + BLUE, + PURPLE, + YELLOW, + 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 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); - - +DigitalOut red_led(LED_RED); +DigitalOut green_led(LED_GREEN); +DigitalOut blue_led(LED_BLUE); +Receiver rcv; +EthernetInterface eth; // PID controller parameters and functions float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k) float P, I, D, N, reference = 0; void controlAnglePID(float P, float I, float D, float N); void initializeController(); +void control(); +Ticker controller_ticker; // Magnetometer variables and functions float max_x, max_y, min_x, min_y,x,y; MotionSensorDataUnits mag_data; +MotionSensorDataCounts mag_raw; float processMagAngle(); void magCal(); +// Protocol +void readProtocol(); + int main(){ - gyro.gyro_config(MODE_2); - gyro.start_measure(GYRO_PERIOD); + // Initializing sensors: + acc.enable(); + //magCal(); + gyro.gyro_config(MODE_1); initializeController(); + + // Set initial control configurations + motor.setVelocity(0); + + // Protocol parameters + set_leds_color(RED, red_led, green_led, blue_led); + eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR); + eth.connect(); + set_leds_color(BLUE, red_led, green_led, blue_led); + rcv.set_socket(); + + gyro.start_measure(GYRO_PERIOD); + controller_ticker.attach(&control,Ts); + //main loop while(1){ - controlAnglePID(P,I,D,N); - printf("%f \r\n",gyro.get_angle()); - wait(Ts); + readProtocol(); + wait(0.01); } } +void control(){ + controlAnglePID(P,I,D,N); +} void readProtocol(){ - char msg = ser.getc(); + if(!rcv.receive()) + return; + char msg = rcv.get_msg(); switch(msg) { case NONE: //ser.printf("sending red signal to led\r\n"); - return; + red_led = LED_ON; + + printf("NONE\r\n"); + + wait(1); + red_led = LED_OFF; break; case BRAKE: //ser.printf("sending green signal to led\r\n"); + green_led = LED_ON; + motor.stopJogging(); + printf("BRAKE\r\n"); motor.brakeMotor(); + green_led = LED_OFF; break; case ANG_RST: //ser.printf("sending blue signal to led\r\n"); + blue_led = LED_ON; + + printf("ANG_RST\r\n"); gyro.stop_measure(); gyro.start_measure(GYRO_PERIOD); - return; + blue_led = LED_OFF; + initializeController(); break; case ANG_REF: - reference = get_ang_ref(ser); + red_led = LED_ON; + green_led = LED_ON; + + reference = rcv.get_ang_ref();// - processMagAngle(); + printf("New reference: %f \n\r",reference*180/PI); + if(reference > PI) + reference = reference - 2*PI; + if(reference < -PI) + reference = reference + 2*PI; + red_led = LED_OFF; + green_led = LED_OFF; break; - case GND_SPEED: - motor.setVelocity(get_gnd_speed(ser)); + case GND_VEL: + red_led = LED_ON; + blue_led = LED_ON; + float vel = rcv.get_gnd_vel(); + motor.setVelocity(vel); + printf("GND_VEL = %f\r\n", vel); + + red_led = LED_OFF; + blue_led = LED_OFF; + break; + case JOG_VEL: + red_led = LED_ON; + blue_led = LED_ON; + + float p, r; + rcv.get_jog_vel(&p,&r); + if(p == 0 || r == 0) + motor.stopJogging(); + else + motor.startJogging(r,p); + red_led = LED_OFF; + blue_led = LED_OFF; break; case PID_PARAMS: - ser.putc('p'); - get_pid_params(ser, &P, &I, &D, &N); + blue_led = LED_ON; + green_led = LED_ON; + + 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]); + + wait(1); + blue_led = LED_OFF; + green_led = LED_OFF; break; default: - // ser.flush(); - + blue_led = LED_ON; + green_led = LED_ON; + red_led = LED_ON; + printf("nothing understood\r\n"); + wait(1); + blue_led = LED_OFF; + green_led = LED_OFF; + red_led = LED_OFF; + //ser.printf("unknown command!\r\n"); } } /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */ @@ -126,6 +278,7 @@ /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */ /* Function to normalize the magnetometer reading */ void magCal(){ + //red_led = 0; printf("Starting Calibration"); mag.enable(); wait(0.01); @@ -155,5 +308,5 @@ mag.getAxis(mag_data); x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1; y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1; - return atan2(y,x); + return -atan2(y,x); } \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Sun May 01 23:01:27 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#7a567bf048bf