Revision 4:62a6681510d6, committed 2016-11-23

Comitter:

kimnielsen

Date:

Wed Nov 23 21:40:26 2016 +0000

Parent:

3:30bdc3d9e1c6

Commit message:

.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometry.h	Wed Nov 23 21:40:26 2016 +0000
@@ -0,0 +1,256 @@
+#ifndef ODOMETRY_H
+#define ODOMETRY_H
+
+#include "mbed.h"
+#include "hack_motor.h"
+#include "math.h"
+
+//  ODOMETRY VALUES
+#define N 20 // ticks on wheel
+#define R 32.5 // radius = 32.5 mm
+#define L 133 // 133 mm distance between wheels
+#define PI 3.141592
+int tickL = 0; // tick on left wheel
+int tickR = 0; // tick on right wheel
+
+//  PID VALUES
+#define P_TERM 1.0
+#define I_TERM 0.1
+#define D_TERM 0
+#define MAX 1.0 
+#define MIN 0
+
+//  GLOBAL DEFINITIONS
+double right,left;
+double speed=0.5;
+
+//  ANALOG POWER
+AnalogIn   ain(PC_4);
+DigitalOut dout(PB_13);
+DigitalOut greenout(PB_12);
+int stop=0;         //DigitalOut DEFINITION OF RED LED!!
+
+//  COORDINATES TO REACH
+const double xPosition [] =
+{
+    [0] = 0,
+    [1] = 1000,
+    [2] = 2000,
+    [3] = 3000,
+    [4] = 4000,
+    [5] = 5000,
+    [6] = 4000,
+    [7] = 3000,
+    [8] = 2000,
+    [9] = 1000,
+    [10] = 2000,
+    [11] = 3000,
+    [12] = 4000,
+    [13] = 5000,
+    [14] = 1000
+};
+const double yPosition [] = 
+{
+    [0] = 0,
+    [1] = 2000,
+    [2] = 3000,
+    [3] = 4000,
+    [4] = 5000,
+    [5] = 2000,
+    [6] = 4000,
+    [7] = 3000,
+    [8] = 2000,
+    [9] = 1000,
+    [10] = 3000,
+    [11] = 2000,
+    [12] = 1000,
+    [13] = 2000,
+    [14] = 3000
+};
+const double THETA [] =
+{
+    [0] = 0.785,
+    [1] = 1.047,
+    [2] = 0.698,
+    [3] = 0.524,
+    [4] = 1.047,
+    [5] = 0.698,
+    [6] = 0.785,
+    [7] = 0.873,
+    [8] = 0.349,
+    [9] = 0.349,
+    [10] = 0.611,
+    [11] = 0.611,
+    [12] = 0.436,
+    [13] = 0.96
+}; //INCLUDE ANGLE TO HEAD BACK TO ORIGIN POSITION!!!!!
+
+const double DISTANCES [] = 
+{
+    [0] = sqrt(pow((xPosition[1] - xPosition[0]), 2) + pow((yPosition[1] - yPosition[0]), 2)),
+    [1] = sqrt(pow((xPosition[2] - xPosition[1]), 2) + pow((yPosition[2] - yPosition[1]), 2)),
+    [2] = sqrt(pow((xPosition[3] - xPosition[2]), 2) + pow((yPosition[3] - yPosition[2]), 2)),
+    [3] = sqrt(pow((xPosition[4] - xPosition[3]), 2) + pow((yPosition[4] - yPosition[3]), 2)),
+    [4] = sqrt(pow((xPosition[5] - xPosition[4]), 2) + pow((yPosition[5] - yPosition[4]), 2)),
+    [5] = sqrt(pow((xPosition[6] - xPosition[5]), 2) + pow((yPosition[6] - yPosition[5]), 2)),
+    [6] = sqrt(pow((xPosition[7] - xPosition[6]), 2) + pow((yPosition[7] - yPosition[6]), 2)),
+    [7] = sqrt(pow((xPosition[8] - xPosition[7]), 2) + pow((yPosition[8] - yPosition[7]), 2)),
+    [8] = sqrt(pow((xPosition[9] - xPosition[8]), 2) + pow((yPosition[9] - yPosition[8]), 2)),
+    [9] = sqrt(pow((xPosition[10] - xPosition[9]), 2) + pow((yPosition[10] - yPosition[9]), 2)),
+    [10] = sqrt(pow((xPosition[11] - xPosition[10]), 2) + pow((yPosition[11] - yPosition[10]), 2)),
+    [11] = sqrt(pow((xPosition[12] - xPosition[11]), 2) + pow((yPosition[12] - yPosition[11]), 2)),
+    [12] = sqrt(pow((xPosition[13] - xPosition[12]), 2) + pow((yPosition[13] - yPosition[12]), 2)),
+    [13] = sqrt(pow((xPosition[14] - xPosition[13]), 2) + pow((yPosition[14] - yPosition[13]), 2))
+};  //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN
+
+/*
+=============================================================================
+TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE
+=============================================================================*/
+Timer t; // DEFINE A TIMER T
+Serial pc(USBTX, USBRX); // not used here because we only have one serial
+// connection
+InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
+InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå
+
+Wheel robot(PB_2, PB_1, PB_15, PB_14);  //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
+// create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
+
+/*      USED FUNCTIONS IN ROBOT.CPP*/
+
+void read_analog() // comes here every second in this case
+// could be flagged with global variables like "stop"
+{
+    if(ain > 0.6f) { // 60% power, time for recharge
+        dout = 1;
+        stop=0;
+    } else {
+        dout = not dout;
+        stop=1; // flash red led
+    }
+
+    if (ain==1.0f) greenout = 1; // full power
+    else greenout = 0;
+}
+
+//  INIT YOUR PARAMETERS
+void init()
+{
+    tickL=0;
+    tickR=0;
+}
+
+//  SENSOR INFO FROM TACO SENSORS
+void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
+{
+    tickL++;
+}
+void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
+{
+    tickR++;
+}
+float Dleft() // DISTANCE FOR LEFT WHEEL
+{
+    return 2*PI*R*tickL/N;
+}
+
+float Dright() // DISTANCE FOR RIGHT WHEEL
+{
+    return 2*PI*R*tickR/N;
+}
+
+float Dcenter() // DISTANCE FOR CENTER
+{
+    return (Dleft()+Dright())/2;
+}
+
+float DeltaTHETA()  //UPDATE ON ANGLE
+{
+    return (Dright()-Dleft())/2*L;
+}
+
+    //  X POSITIONS
+float DeltaX_1()    //  1. X POSITION
+{
+    return Dcenter()*cos(THETA[0]+DeltaTHETA()/2);
+}
+
+    //  Y POSITIONS
+float DeltaY_1()    //1. Y POSITION
+{
+    return Dcenter()*sin(THETA[0]+DeltaTHETA()/2);
+}
+    //  DISTANCES TRAVELLED 
+float Dtravel_1()   //  1. DISTANCE
+{
+    return sqrt(pow(DeltaX_1(), 2) + pow(DeltaY_1(), 2));
+}
+
+    //  PID REGULATOR
+void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CONSIDERATION
+{   
+    double e = 0; // angle error
+    double THETA_D1 = atan((yPosition[1]-yPosition[0])/(xPosition[1]-xPosition[0]));    //  THETA DESIRED CALCULATED
+    double output = 0;
+    double derivative = 0;
+    double proportional = 0;
+    double integral = 0;
+    double e_old = 0;   
+
+    while (Dtravel_1() <= DISTANCES[0])    //  INDSTIL TIL RIGTIGE PUNKTER!!! SKER I ET PARALLELT TIDSFORLØB MED KØRSEL AF ROBOT TIL PUNKTER
+    {
+        if(THETA_D1 < -PI)    //  EVALUATES IF THE ANGLE IS LESS THAN -3.14
+        {
+            THETA_D1 = -PI;
+        }
+    
+        else if(THETA_D1 > PI)  //  EVALUATES IF THE ANGLE IS MORE THAN 3.14
+        {
+            THETA_D1 = PI;   
+        }
+        
+        e = THETA_D1 - THETA[0];    //  ERROR VALUE
+
+        // Compute the proportional
+        proportional = e; // get the error
+
+        // Compute the integral
+        integral += proportional;
+
+        // Compute the derivative
+        derivative = e - e_old;
+
+        // Remember the last error.
+        e_old = e;
+
+        // Compute the output
+        output = (proportional * (P_TERM)) + (integral * (I_TERM))
+                + (derivative * (D_TERM));
+
+        // Compute new speeds of both wheels
+        right = speed - output;//if power is 0, no error and same speed on wheels
+        left = speed + output;
+
+        // limit checks
+        if (right < MIN)
+            right = MIN;
+        else if (right > MAX)
+            right = MAX;
+
+        if (left < MIN)
+            left = MIN;
+        else if (left > MAX)
+            left = MAX;
+
+        robot.FW(right,left); // set new positions
+        printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
+               Dcenter(),tickL,tickR,left,right);
+               
+        printf("\n\r Error: %.2lf", e); //  HUSK DETTE!
+
+        wait_ms(20); // should be adjusted to your context. Give the motor time
+        // to do something before you react
+    }
+    t.stop(); // stop timer
+}
+#endif
\ No newline at end of file

--- a/robot.cpp	Wed Nov 02 08:11:24 2016 +0000
+++ b/robot.cpp	Wed Nov 23 21:40:26 2016 +0000
@@ -5,128 +5,48 @@
  Version     : 0.1
  Date        : 13-10-2016
  Copyright   : Open for all
- Description : Program to serve af platform for Pro1 2016
+ Description : Program to serve the platform for Pro1 2016
  ============================================================================
  */
-    
-/*
-=============================================================================
-Including the necessary header files
-=============================================================================
-*/
+#include "odometry.h"
 #include "mbed.h"
 #include "HCSR04.h"
 #include "nucleo_servo.h"
 #include "hack_motor.h"
 #include <math.h> 
 
-/*
-=============================================================================
-PID definitions
-=============================================================================
-*/
-#define P_TERM 0.2
-#define I_TERM 0.1
-#define D_TERM 0.3
-#define MAX 1.0 
-#define MIN 0
-#define PI 3.141592
-
-/*
-=============================================================================
-Geometry for robot:
-note that N,R,L maybe not have the most meaningful name
-but they follow the names from the theory for our
-robot lectures, and can be used in youa application
-=============================================================================
-*/         
-#define N 20 // ticks on wheel
-#define R 32.5 // radius = 32.5 mm
-#define L 133 // 133 mm distance between wheels
- 
-/*
-=============================================================================
-Coordinates for robot to reach
-=============================================================================
-*/
-//Start position:
-#define first_x 0
-#define first_y 0
-#define first_angle 0.785  //45 deg
-
-//Second position:
-#define second_x 50
-#define second_y 40
-#define second_angle 1.047 //60 deg
-
-//Third position:
-#define third_x 70
-#define third_y 10
-#define third_angle 1.396 //80 deg
-
-//fourth position:
-#define fourth_x 70
-#define fourth_y 50
-  
-Timer t; // define a timer t
-Serial pc(USBTX, USBRX); // not used here because we only have one serial
-// connection
-InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
-InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
-
-Wheel robot(PB_2, PB_1, PB_15, PB_14);  
-// create an object of robot H-bridge
-// (M1A, M1B, M2A, M2B)
-
-/*
-=============================================================================
-definition for analog power
-=============================================================================
-*/
-AnalogIn   ain(PC_4);
-DigitalOut dout(PB_13);
-DigitalOut greenout(PB_12);
-
-int stop=0;
-/*
-=============================================================================
-Global variables
-=============================================================================
-*/    
-double right,left;
-double speed=0.5;
-double e = 0; // angle error
-int tickL = 0; // tick on left wheel
-int tickR = 0; // tick on right wheel
-
-/*
-=============================================================================
-Prototype defined functions
-=============================================================================
-*/ 
-void init();
-void tickLeft(); // read tick left
-void tickRight(); // read tick right
-float Dleft(); // distance left wheel
-float Dright(); // distance right wheel
-float Dcenter(); // Distance for center
-
-//functions for calculating desired distances from defined points
-float Dist_reach1();
-float Dist_reach2();
-float Dist_reach3();
-
-void get_to_goal ( ); // function to get to goal
-void read_analog(); // comes here every second
-
+void startfunction( );
 Ticker T1; // create an object T1 of Ticker
 
 /*
 ----------------------------------------------------------------------------
-                            Accessing the int main
+                                MAIN FUNCTION
+----------------------------------------------------------------------------*/
+int main()
+{
+/*
+=============================================================================
+Driving and stopping the robot with member functions from the wheel class
+=============================================================================*/
+    startfunction();    //  1. DRIVE  
+    while(Dtravel_1() <= DISTANCES[0])  //  OVERVEJ AT SMIDE "get_to_goal" funktionen ind i selve while loopet, sådan, at beregningerne sker parallelt. 
+    {
+        get_to_goal(); //   TJEK DETTE
+        robot.FW(0.5, 0.5);
+        wait_ms(5000);
+    }
+    
+    robot.stop(); 
+    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d \n", t.read_ms(),tickL,
+           tickR);   
+    wait_ms(3000);
+} 
+/*  
 ----------------------------------------------------------------------------
-*/
-int main()
+                            END OF MAIN FUNCTION
+----------------------------------------------------------------------------*/
+
+void startfunction()
 {
     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
     //function to read analog in every second
@@ -145,8 +65,7 @@
 /*
 =============================================================================
 Demo of the servor and ulta sonic sensor
-=============================================================================
-*/  
+=============================================================================*/  
     wait_ms(2000); // just get time for you to enable your screeen
     servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
     wait_ms (500);
@@ -164,183 +83,4 @@
 
     wait_ms(1000); //wait 1 secs here before you go
     t.start();      // start timer (just demo of how you can use a timer)
-
-    /*  ------------------
-            with PID
-        -----------------
-    --  delete get_to_goal(); when you use without pid
-    get_to_goal ();
-    */
-
-    /* ---------------------------
-           without pid
-       ------------------------
-    */
-    // delete between --- when you use it with pid
-    // ------------------------------------------------------------
-/*
-=============================================================================
-Driving and stopping the robot with member functions from the wheel class
-=============================================================================
-*/
-    while(Dcenter() <= Dist_reach1())
-    {
-        robot.FW(0.5, 0.5);
-        wait_ms(5000);
-    }
-    robot.stop(); // stop the robot again
-    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
-           tickR);    
-           
-} 
-/*  
-----------------------------------------------------------------------------
-                            END OF MAIN FUNCTION
-----------------------------------------------------------------------------
-*/
-
-/*
-=============================================================================
-Calculations of distances traveled by both wheels and robot
-=============================================================================
-*/
-void tickLeft() // udtate left tick on tacho interrupt
-{
-    tickL++;
-}
-void tickRight() // udtate right tick on tacho interrupt
-{
-    tickR++;
-}
-float Dleft() // Distance for left wheel
-{
-    return 2*PI*R*tickL/N;
-}
-
-float Dright() // Distance for right wheel
-{
-    return 2*PI*R*tickR/N;
-}
-
-float Dcenter() // Distance for center
-{
-    return (Dleft()+Dright())/2;
-}
-
-//Calculation of current position and new position with angles!!!
-
-float angle_new()
-{
-    return (Dright()-Dleft())/L;   
-}
-
-float x_position()
-{
-    return Dcenter()*sin(angle_new());   
-}
-
-float y_position()
-{
-    return Dcenter()*cos(angle_new());
-}
-
-//slet efterfølgende
-float Dist_reach1() 
-{ 
-  double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
-  double dist_firstTosecond = sqrt(firstTosecond_calc); 
-  
-  return (dist_firstTosecond);
-}
-
-/*
-=============================================================================
-PID init
-=============================================================================
-*/
-void get_to_goal ( ) // function to get to goal
-{
-    double power = 0;
-    double derivative = 0;
-    double proportional = 0;
-    double integral = 0;
-    double e_old = 0;
-
-    while (Dcenter() < Dist_reach1()) {
-/*
--------------------------------------------------
-error is difference between right tick and left tick
-should be ajusted to your context, angle for robot
-and instead of test Dcenter() in your while loop, test
-for distance_to_goal()
--------------------------------------------------
-*/
-        e = tickR-tickL; 
-/*
-=============================================================================
-PID regulator
-=============================================================================
-*/
-
-        // Compute the proportional
-        proportional = e; // get the error
-
-        // Compute the integral
-        integral += proportional;
-
-        // Compute the derivative
-        derivative = e - e_old;
-
-        // Remember the last error.
-        e_old = e;
-
-        // Compute the power
-        power = (proportional * (P_TERM)) + (integral * (I_TERM))
-                + (derivative * (D_TERM));
-
-        // Compute new speeds
-
-        right = speed - power;//if power is 0, no error and same speed on wheels
-        left = speed + power;
-
-
-        // limit checks
-        if (right < MIN)
-            right = MIN;
-        else if (right > MAX)
-            right = MAX;
-
-        if (left < MIN)
-            left = MIN;
-        else if (left > MAX)
-            left = MAX;
-
-        robot.FW(right,left); // set new positions
-        printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
-               Dcenter(),tickL,tickR,left,right);
-
-        wait_ms(20); // should be adjusted to your context. Give the motor time
-        // to do something before you react
-    }
-    t.stop(); // stop timer
-}
-
-void read_analog() // comes here every second in this case
-// could be flagged with global variables like "stop"
-{
-    if(ain > 0.6f) { // 60% power, time for recharge
-        dout = 1;
-        stop=0;
-    } else {
-        dout = not dout;
-        stop=1; // flash red led
-    }
-
-    if (ain==1.0f) greenout = 1; // full power
-    else greenout = 0;
-}
-void init() // init your parameters
-{
-    tickL=0;
-    tickR=0;
 }
\ No newline at end of file