Kildekode
Fork of Team5_kode by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 02:32:05 by
1.7.2
