Diff: main.cpp

Revision:

14:e8cd237c8639

Parent:

13:f7a7fe9b5c00

Child:

16:a9e0eb97557f

--- 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