odometry.h

00001 #ifndef ODOMETRY_H
00002 #define ODOMETRY_H
00003 #include "mbed.h"
00004 #include "hack_motor.h"
00005 #include "math.h"   
00006 
00007 /////////////////////////////////////////////////////////////////////////////
00008 //  ODOMETRY VALUES                                                        //
00009 /////////////////////////////////////////////////////////////////////////////
00010 #define N 20 // ticks on wheel
00011 #define R 33.5 // radius = 33.5 mm on wheels
00012 #define L 133 // 133 mm distance between wheels
00013 #define PI 3.141592
00014 int tickL = 0; // tick on left wheel
00015 int tickR = 0; // tick on right wheel
00016 
00017 /////////////////////////////////////////////////////////////////////////////
00018 //  PID VALUES                                                             //
00019 /////////////////////////////////////////////////////////////////////////////
00020 #define P_TERM 0.13    
00021 #define I_TERM 0
00022 #define D_TERM 0
00023 
00024 /////////////////////////////////////////////////////////////////////////////
00025 //  GLOBAL DEFINITIONS                                                     //
00026 /////////////////////////////////////////////////////////////////////////////
00027 double right,left;  
00028 double result;
00029 double speed=0.5;
00030 double distance = 4500;
00031 /////////////////////////////////////////////////////////////////////////////
00032 //  ANALOG POWER                                                           //
00033 /////////////////////////////////////////////////////////////////////////////
00034 AnalogIn   ain(PC_4);
00035 DigitalOut dout(PB_13);
00036 DigitalOut greenout(PB_12);
00037 int stop=0;                 //DigitalOut DEFINITION OF RED LED!!
00038 DigitalOut LedFlash(PA_3);  //  RED LED WILL FLASH IF ROBOT STOPS 
00039 
00040 /////////////////////////////////////////////////////////////////////////////
00041 //              TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE               //
00042 /////////////////////////////////////////////////////////////////////////////
00043 Timer t; // DEFINE A TIMER T
00044 Serial pc(USBTX, USBRX); // not used here because we only have one serial 
00045 // connection
00046 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left
00047 InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right
00048 Wheel robot(PB_2, PB_1, PB_15, PB_14); 
00049 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
00050 
00051     //  GETTING INFO FROM SENSORS 
00052 void read_analog() // comes here every second in this case
00053 // could be flagged with global variables like "stop"
00054 {
00055     if(ain > 0.6f) { // 60% power, time for recharge
00056         dout = 1;
00057         stop=0; 
00058     } else {
00059         dout = not dout;
00060         stop=1; // flash red led
00061     }
00062     if (ain==1.0f) greenout = 1; // full power
00063     else greenout = 0;
00064 }
00065 //  INIT YOUR PARAMETERS
00066 void init()                          
00067 {
00068     tickL=0;
00069     tickR=0;
00070 }
00071 //  SENSOR INFO FROM TACO SENSORS
00072 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
00073 {
00074     tickL++;
00075 }
00076 
00077 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
00078 {
00079     tickR++;
00080 }
00081     
00082 float Dleft() // DISTANCE FOR LEFT WHEEL
00083 {
00084     return (2*PI*R*tickL)/N;
00085 }
00086 
00087 float Dright() // DISTANCE FOR RIGHT WHEEL
00088 {
00089     return (2*PI*R*tickR)/N;
00090 }
00091 
00092 float Dcenter() // DISTANCE FOR CENTER
00093 {
00094     return (Dleft()+Dright())/2;
00095 }
00096     
00097 /////////////////////////////////////////////////////////////////////
00098 //                      PID REGULATOR                              //
00099 /////////////////////////////////////////////////////////////////////
00100 void get_to_goal ( )    
00101 {   
00102     double e = 0; 
00103     double output = 0;
00104     double derivative = 0;
00105     double proportional = 0;
00106     double integral = 0;
00107     double e_old = 0;
00108 
00109     while (Dcenter() <= distance) 
00110     {
00111         right = speed;     
00112         left = speed;
00113         e = right;      
00114         
00115         //PID calculation
00116         proportional = e; // GETTING THE ERROR VALUE
00117         integral += proportional;   // THE ERROR NEVER GETS TOO LOW
00118         derivative = e - e_old;
00119         e_old = e;
00120         
00121         // COMPUTING THE OUTPUT SIGNAL
00122         output = (proportional * (P_TERM)) + (integral * (I_TERM))+ 
00123         (derivative * (D_TERM));
00124         
00125         // COMPUTING NEW SPEEDS ON WHEELS
00126         right = speed - output;
00127         left = speed + output*0.15; 
00128         
00129         robot.FW(right,left);
00130         printf("tickR: %d   rightSpeed: %.2lf   tickL: %d   leftSpeed: %.2lf",
00131         tickR, right, tickL, left);
00132         wait_ms(10); // should be adjusted to your context. Give the motor time
00133         // to do something before you react
00134     }
00135     t.stop(); // stop timer
00136 } 
00137 #endif