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