Kildekode

Dependencies:   mbed

Fork of Team5_kode by E2016-S1-Team5

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers odometry.h Source File

odometry.h

00001 #ifndef ODOMETRY_H
00002 #define ODOMETRY_H
00003 
00004 #include "mbed.h"
00005 #include "hack_motor.h"
00006 #include "math.h"
00007 
00008 //  ODOMETRY VALUES
00009 #define N 20 // ticks on wheel
00010 #define R 32.5 // radius = 32.5 mm
00011 #define L 133 // 133 mm distance between wheels
00012 #define PI 3.141592
00013 int tickL = 0; // tick on left wheel
00014 int tickR = 0; // tick on right wheel
00015 
00016 //  PID VALUES
00017 #define P_TERM 1.0
00018 #define I_TERM 0.1
00019 #define D_TERM 0
00020 #define MAX 1.0 
00021 #define MIN 0
00022 
00023 //  GLOBAL DEFINITIONS
00024 double right,left;
00025 double speed=0.5;
00026 
00027 //  ANALOG POWER
00028 AnalogIn   ain(PC_4);
00029 DigitalOut dout(PB_13);
00030 DigitalOut greenout(PB_12);
00031 int stop=0;         //DigitalOut DEFINITION OF RED LED!!
00032 
00033 //  COORDINATES TO REACH
00034 const double xPosition [] =
00035 {
00036     [0] = 0,
00037     [1] = 1000,
00038     [2] = 2000,
00039     [3] = 3000,
00040     [4] = 4000,
00041     [5] = 5000,
00042     [6] = 4000,
00043     [7] = 3000,
00044     [8] = 2000,
00045     [9] = 1000,
00046     [10] = 2000,
00047     [11] = 3000,
00048     [12] = 4000,
00049     [13] = 5000,
00050     [14] = 1000
00051 };
00052 const double yPosition [] = 
00053 {
00054     [0] = 0,
00055     [1] = 2000,
00056     [2] = 3000,
00057     [3] = 4000,
00058     [4] = 5000,
00059     [5] = 2000,
00060     [6] = 4000,
00061     [7] = 3000,
00062     [8] = 2000,
00063     [9] = 1000,
00064     [10] = 3000,
00065     [11] = 2000,
00066     [12] = 1000,
00067     [13] = 2000,
00068     [14] = 3000
00069 };
00070 const double THETA [] =
00071 {
00072     [0] = 0.785,
00073     [1] = 1.047,
00074     [2] = 0.698,
00075     [3] = 0.524,
00076     [4] = 1.047,
00077     [5] = 0.698,
00078     [6] = 0.785,
00079     [7] = 0.873,
00080     [8] = 0.349,
00081     [9] = 0.349,
00082     [10] = 0.611,
00083     [11] = 0.611,
00084     [12] = 0.436,
00085     [13] = 0.96
00086 }; //INCLUDE ANGLE TO HEAD BACK TO ORIGIN POSITION!!!!!
00087 
00088 const double DISTANCES [] = 
00089 {
00090     [0] = sqrt(pow((xPosition[1] - xPosition[0]), 2) + pow((yPosition[1] - yPosition[0]), 2)),
00091     [1] = sqrt(pow((xPosition[2] - xPosition[1]), 2) + pow((yPosition[2] - yPosition[1]), 2)),
00092     [2] = sqrt(pow((xPosition[3] - xPosition[2]), 2) + pow((yPosition[3] - yPosition[2]), 2)),
00093     [3] = sqrt(pow((xPosition[4] - xPosition[3]), 2) + pow((yPosition[4] - yPosition[3]), 2)),
00094     [4] = sqrt(pow((xPosition[5] - xPosition[4]), 2) + pow((yPosition[5] - yPosition[4]), 2)),
00095     [5] = sqrt(pow((xPosition[6] - xPosition[5]), 2) + pow((yPosition[6] - yPosition[5]), 2)),
00096     [6] = sqrt(pow((xPosition[7] - xPosition[6]), 2) + pow((yPosition[7] - yPosition[6]), 2)),
00097     [7] = sqrt(pow((xPosition[8] - xPosition[7]), 2) + pow((yPosition[8] - yPosition[7]), 2)),
00098     [8] = sqrt(pow((xPosition[9] - xPosition[8]), 2) + pow((yPosition[9] - yPosition[8]), 2)),
00099     [9] = sqrt(pow((xPosition[10] - xPosition[9]), 2) + pow((yPosition[10] - yPosition[9]), 2)),
00100     [10] = sqrt(pow((xPosition[11] - xPosition[10]), 2) + pow((yPosition[11] - yPosition[10]), 2)),
00101     [11] = sqrt(pow((xPosition[12] - xPosition[11]), 2) + pow((yPosition[12] - yPosition[11]), 2)),
00102     [12] = sqrt(pow((xPosition[13] - xPosition[12]), 2) + pow((yPosition[13] - yPosition[12]), 2)),
00103     [13] = sqrt(pow((xPosition[14] - xPosition[13]), 2) + pow((yPosition[14] - yPosition[13]), 2))
00104 };  //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN
00105 
00106 /*
00107 =============================================================================
00108 TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE
00109 =============================================================================*/
00110 Timer t; // DEFINE A TIMER T
00111 Serial pc(USBTX, USBRX); // not used here because we only have one serial
00112 // connection
00113 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
00114 InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå
00115 
00116 Wheel robot(PB_2, PB_1, PB_15, PB_14);  //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
00117 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
00118 
00119 /*      USED FUNCTIONS IN ROBOT.CPP*/
00120 
00121 void read_analog() // comes here every second in this case
00122 // could be flagged with global variables like "stop"
00123 {
00124     if(ain > 0.6f) { // 60% power, time for recharge
00125         dout = 1;
00126         stop=0;
00127     } else {
00128         dout = not dout;
00129         stop=1; // flash red led
00130     }
00131 
00132     if (ain==1.0f) greenout = 1; // full power
00133     else greenout = 0;
00134 }
00135 
00136 //  INIT YOUR PARAMETERS
00137 void init()
00138 {
00139     tickL=0;
00140     tickR=0;
00141 }
00142 
00143 //  SENSOR INFO FROM TACO SENSORS
00144 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
00145 {
00146     tickL++;
00147 }
00148 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
00149 {
00150     tickR++;
00151 }
00152 float Dleft() // DISTANCE FOR LEFT WHEEL
00153 {
00154     return 2*PI*R*tickL/N;
00155 }
00156 
00157 float Dright() // DISTANCE FOR RIGHT WHEEL
00158 {
00159     return 2*PI*R*tickR/N;
00160 }
00161 
00162 float Dcenter() // DISTANCE FOR CENTER
00163 {
00164     return (Dleft()+Dright())/2;
00165 }
00166 
00167 float DeltaTHETA()  //UPDATE ON ANGLE
00168 {
00169     return (Dright()-Dleft())/2*L;
00170 }
00171 
00172     //  X POSITIONS
00173 float DeltaX_1()    //  1. X POSITION
00174 {
00175     return Dcenter()*cos(THETA[0]+DeltaTHETA()/2);
00176 }
00177 
00178     //  Y POSITIONS
00179 float DeltaY_1()    //1. Y POSITION
00180 {
00181     return Dcenter()*sin(THETA[0]+DeltaTHETA()/2);
00182 }
00183     //  DISTANCES TRAVELLED 
00184 float Dtravel_1()   //  1. DISTANCE
00185 {
00186     return sqrt(pow(DeltaX_1(), 2) + pow(DeltaY_1(), 2));
00187 }
00188 
00189     //  PID REGULATOR
00190 void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CONSIDERATION
00191 {   
00192     double e = 0; // angle error
00193     double THETA_D1 = atan((yPosition[1]-yPosition[0])/(xPosition[1]-xPosition[0]));    //  THETA DESIRED CALCULATED
00194     double output = 0;
00195     double derivative = 0;
00196     double proportional = 0;
00197     double integral = 0;
00198     double e_old = 0;   
00199 
00200     while (Dtravel_1() <= DISTANCES[0])    //  INDSTIL TIL RIGTIGE PUNKTER!!! SKER I ET PARALLELT TIDSFORLØB MED KØRSEL AF ROBOT TIL PUNKTER
00201     {
00202         if(THETA_D1 < -PI)    //  EVALUATES IF THE ANGLE IS LESS THAN -3.14
00203         {
00204             THETA_D1 = -PI;
00205         }
00206     
00207         else if(THETA_D1 > PI)  //  EVALUATES IF THE ANGLE IS MORE THAN 3.14
00208         {
00209             THETA_D1 = PI;   
00210         }
00211         
00212         e = THETA_D1 - THETA[0];    //  ERROR VALUE
00213 
00214         // Compute the proportional
00215         proportional = e; // get the error
00216 
00217         // Compute the integral
00218         integral += proportional;
00219 
00220         // Compute the derivative
00221         derivative = e - e_old;
00222 
00223         // Remember the last error.
00224         e_old = e;
00225 
00226         // Compute the output
00227         output = (proportional * (P_TERM)) + (integral * (I_TERM))
00228                 + (derivative * (D_TERM));
00229 
00230         // Compute new speeds of both wheels
00231         right = speed - output;//if power is 0, no error and same speed on wheels
00232         left = speed + output;
00233 
00234         // limit checks
00235         if (right < MIN)
00236             right = MIN;
00237         else if (right > MAX)
00238             right = MAX;
00239 
00240         if (left < MIN)
00241             left = MIN;
00242         else if (left > MAX)
00243             left = MAX;
00244 
00245         robot.FW(right,left); // set new positions
00246         printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
00247                Dcenter(),tickL,tickR,left,right);
00248                
00249         printf("\n\r Error: %.2lf", e); //  HUSK DETTE!
00250 
00251         wait_ms(20); // should be adjusted to your context. Give the motor time
00252         // to do something before you react
00253     }
00254     t.stop(); // stop timer
00255 }
00256 #endif