E2016-S1-Team5 / Mbed 2 deprecated Team5

Kildekode til robot

Dependencies:   PID mbed

Fork of Robotkode by Kim Nielsen

odometry.h@4:62a6681510d6, 2016-11-23 (annotated)

Committer:
kimnielsen
Date:
Wed Nov 23 21:40:26 2016 +0000
Revision:
4:62a6681510d6
Child:
5:cf033e9d4468
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimnielsen 4:62a6681510d6 1 #ifndef ODOMETRY_H
kimnielsen 4:62a6681510d6 2 #define ODOMETRY_H
kimnielsen 4:62a6681510d6 3
kimnielsen 4:62a6681510d6 4 #include "mbed.h"
kimnielsen 4:62a6681510d6 5 #include "hack_motor.h"
kimnielsen 4:62a6681510d6 6 #include "math.h"
kimnielsen 4:62a6681510d6 7
kimnielsen 4:62a6681510d6 8 // ODOMETRY VALUES
kimnielsen 4:62a6681510d6 9 #define N 20 // ticks on wheel
kimnielsen 4:62a6681510d6 10 #define R 32.5 // radius = 32.5 mm
kimnielsen 4:62a6681510d6 11 #define L 133 // 133 mm distance between wheels
kimnielsen 4:62a6681510d6 12 #define PI 3.141592
kimnielsen 4:62a6681510d6 13 int tickL = 0; // tick on left wheel
kimnielsen 4:62a6681510d6 14 int tickR = 0; // tick on right wheel
kimnielsen 4:62a6681510d6 15
kimnielsen 4:62a6681510d6 16 // PID VALUES
kimnielsen 4:62a6681510d6 17 #define P_TERM 1.0
kimnielsen 4:62a6681510d6 18 #define I_TERM 0.1
kimnielsen 4:62a6681510d6 19 #define D_TERM 0
kimnielsen 4:62a6681510d6 20 #define MAX 1.0
kimnielsen 4:62a6681510d6 21 #define MIN 0
kimnielsen 4:62a6681510d6 22
kimnielsen 4:62a6681510d6 23 // GLOBAL DEFINITIONS
kimnielsen 4:62a6681510d6 24 double right,left;
kimnielsen 4:62a6681510d6 25 double speed=0.5;
kimnielsen 4:62a6681510d6 26
kimnielsen 4:62a6681510d6 27 // ANALOG POWER
kimnielsen 4:62a6681510d6 28 AnalogIn ain(PC_4);
kimnielsen 4:62a6681510d6 29 DigitalOut dout(PB_13);
kimnielsen 4:62a6681510d6 30 DigitalOut greenout(PB_12);
kimnielsen 4:62a6681510d6 31 int stop=0; //DigitalOut DEFINITION OF RED LED!!
kimnielsen 4:62a6681510d6 32
kimnielsen 4:62a6681510d6 33 // COORDINATES TO REACH
kimnielsen 4:62a6681510d6 34 const double xPosition [] =
kimnielsen 4:62a6681510d6 35 {
kimnielsen 4:62a6681510d6 36 [0] = 0,
kimnielsen 4:62a6681510d6 37 [1] = 1000,
kimnielsen 4:62a6681510d6 38 [2] = 2000,
kimnielsen 4:62a6681510d6 39 [3] = 3000,
kimnielsen 4:62a6681510d6 40 [4] = 4000,
kimnielsen 4:62a6681510d6 41 [5] = 5000,
kimnielsen 4:62a6681510d6 42 [6] = 4000,
kimnielsen 4:62a6681510d6 43 [7] = 3000,
kimnielsen 4:62a6681510d6 44 [8] = 2000,
kimnielsen 4:62a6681510d6 45 [9] = 1000,
kimnielsen 4:62a6681510d6 46 [10] = 2000,
kimnielsen 4:62a6681510d6 47 [11] = 3000,
kimnielsen 4:62a6681510d6 48 [12] = 4000,
kimnielsen 4:62a6681510d6 49 [13] = 5000,
kimnielsen 4:62a6681510d6 50 [14] = 1000
kimnielsen 4:62a6681510d6 51 };
kimnielsen 4:62a6681510d6 52 const double yPosition [] =
kimnielsen 4:62a6681510d6 53 {
kimnielsen 4:62a6681510d6 54 [0] = 0,
kimnielsen 4:62a6681510d6 55 [1] = 2000,
kimnielsen 4:62a6681510d6 56 [2] = 3000,
kimnielsen 4:62a6681510d6 57 [3] = 4000,
kimnielsen 4:62a6681510d6 58 [4] = 5000,
kimnielsen 4:62a6681510d6 59 [5] = 2000,
kimnielsen 4:62a6681510d6 60 [6] = 4000,
kimnielsen 4:62a6681510d6 61 [7] = 3000,
kimnielsen 4:62a6681510d6 62 [8] = 2000,
kimnielsen 4:62a6681510d6 63 [9] = 1000,
kimnielsen 4:62a6681510d6 64 [10] = 3000,
kimnielsen 4:62a6681510d6 65 [11] = 2000,
kimnielsen 4:62a6681510d6 66 [12] = 1000,
kimnielsen 4:62a6681510d6 67 [13] = 2000,
kimnielsen 4:62a6681510d6 68 [14] = 3000
kimnielsen 4:62a6681510d6 69 };
kimnielsen 4:62a6681510d6 70 const double THETA [] =
kimnielsen 4:62a6681510d6 71 {
kimnielsen 4:62a6681510d6 72 [0] = 0.785,
kimnielsen 4:62a6681510d6 73 [1] = 1.047,
kimnielsen 4:62a6681510d6 74 [2] = 0.698,
kimnielsen 4:62a6681510d6 75 [3] = 0.524,
kimnielsen 4:62a6681510d6 76 [4] = 1.047,
kimnielsen 4:62a6681510d6 77 [5] = 0.698,
kimnielsen 4:62a6681510d6 78 [6] = 0.785,
kimnielsen 4:62a6681510d6 79 [7] = 0.873,
kimnielsen 4:62a6681510d6 80 [8] = 0.349,
kimnielsen 4:62a6681510d6 81 [9] = 0.349,
kimnielsen 4:62a6681510d6 82 [10] = 0.611,
kimnielsen 4:62a6681510d6 83 [11] = 0.611,
kimnielsen 4:62a6681510d6 84 [12] = 0.436,
kimnielsen 4:62a6681510d6 85 [13] = 0.96
kimnielsen 4:62a6681510d6 86 }; //INCLUDE ANGLE TO HEAD BACK TO ORIGIN POSITION!!!!!
kimnielsen 4:62a6681510d6 87
kimnielsen 4:62a6681510d6 88 const double DISTANCES [] =
kimnielsen 4:62a6681510d6 89 {
kimnielsen 4:62a6681510d6 90 [0] = sqrt(pow((xPosition[1] - xPosition[0]), 2) + pow((yPosition[1] - yPosition[0]), 2)),
kimnielsen 4:62a6681510d6 91 [1] = sqrt(pow((xPosition[2] - xPosition[1]), 2) + pow((yPosition[2] - yPosition[1]), 2)),
kimnielsen 4:62a6681510d6 92 [2] = sqrt(pow((xPosition[3] - xPosition[2]), 2) + pow((yPosition[3] - yPosition[2]), 2)),
kimnielsen 4:62a6681510d6 93 [3] = sqrt(pow((xPosition[4] - xPosition[3]), 2) + pow((yPosition[4] - yPosition[3]), 2)),
kimnielsen 4:62a6681510d6 94 [4] = sqrt(pow((xPosition[5] - xPosition[4]), 2) + pow((yPosition[5] - yPosition[4]), 2)),
kimnielsen 4:62a6681510d6 95 [5] = sqrt(pow((xPosition[6] - xPosition[5]), 2) + pow((yPosition[6] - yPosition[5]), 2)),
kimnielsen 4:62a6681510d6 96 [6] = sqrt(pow((xPosition[7] - xPosition[6]), 2) + pow((yPosition[7] - yPosition[6]), 2)),
kimnielsen 4:62a6681510d6 97 [7] = sqrt(pow((xPosition[8] - xPosition[7]), 2) + pow((yPosition[8] - yPosition[7]), 2)),
kimnielsen 4:62a6681510d6 98 [8] = sqrt(pow((xPosition[9] - xPosition[8]), 2) + pow((yPosition[9] - yPosition[8]), 2)),
kimnielsen 4:62a6681510d6 99 [9] = sqrt(pow((xPosition[10] - xPosition[9]), 2) + pow((yPosition[10] - yPosition[9]), 2)),
kimnielsen 4:62a6681510d6 100 [10] = sqrt(pow((xPosition[11] - xPosition[10]), 2) + pow((yPosition[11] - yPosition[10]), 2)),
kimnielsen 4:62a6681510d6 101 [11] = sqrt(pow((xPosition[12] - xPosition[11]), 2) + pow((yPosition[12] - yPosition[11]), 2)),
kimnielsen 4:62a6681510d6 102 [12] = sqrt(pow((xPosition[13] - xPosition[12]), 2) + pow((yPosition[13] - yPosition[12]), 2)),
kimnielsen 4:62a6681510d6 103 [13] = sqrt(pow((xPosition[14] - xPosition[13]), 2) + pow((yPosition[14] - yPosition[13]), 2))
kimnielsen 4:62a6681510d6 104 }; //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN
kimnielsen 4:62a6681510d6 105
kimnielsen 4:62a6681510d6 106 /*
kimnielsen 4:62a6681510d6 107 =============================================================================
kimnielsen 4:62a6681510d6 108 TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE
kimnielsen 4:62a6681510d6 109 =============================================================================*/
kimnielsen 4:62a6681510d6 110 Timer t; // DEFINE A TIMER T
kimnielsen 4:62a6681510d6 111 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 4:62a6681510d6 112 // connection
kimnielsen 4:62a6681510d6 113 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
kimnielsen 4:62a6681510d6 114 InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå
kimnielsen 4:62a6681510d6 115
kimnielsen 4:62a6681510d6 116 Wheel robot(PB_2, PB_1, PB_15, PB_14); //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
kimnielsen 4:62a6681510d6 117 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
kimnielsen 4:62a6681510d6 118
kimnielsen 4:62a6681510d6 119 /* USED FUNCTIONS IN ROBOT.CPP*/
kimnielsen 4:62a6681510d6 120
kimnielsen 4:62a6681510d6 121 void read_analog() // comes here every second in this case
kimnielsen 4:62a6681510d6 122 // could be flagged with global variables like "stop"
kimnielsen 4:62a6681510d6 123 {
kimnielsen 4:62a6681510d6 124 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 4:62a6681510d6 125 dout = 1;
kimnielsen 4:62a6681510d6 126 stop=0;
kimnielsen 4:62a6681510d6 127 } else {
kimnielsen 4:62a6681510d6 128 dout = not dout;
kimnielsen 4:62a6681510d6 129 stop=1; // flash red led
kimnielsen 4:62a6681510d6 130 }
kimnielsen 4:62a6681510d6 131
kimnielsen 4:62a6681510d6 132 if (ain==1.0f) greenout = 1; // full power
kimnielsen 4:62a6681510d6 133 else greenout = 0;
kimnielsen 4:62a6681510d6 134 }
kimnielsen 4:62a6681510d6 135
kimnielsen 4:62a6681510d6 136 // INIT YOUR PARAMETERS
kimnielsen 4:62a6681510d6 137 void init()
kimnielsen 4:62a6681510d6 138 {
kimnielsen 4:62a6681510d6 139 tickL=0;
kimnielsen 4:62a6681510d6 140 tickR=0;
kimnielsen 4:62a6681510d6 141 }
kimnielsen 4:62a6681510d6 142
kimnielsen 4:62a6681510d6 143 // SENSOR INFO FROM TACO SENSORS
kimnielsen 4:62a6681510d6 144 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 145 {
kimnielsen 4:62a6681510d6 146 tickL++;
kimnielsen 4:62a6681510d6 147 }
kimnielsen 4:62a6681510d6 148 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 149 {
kimnielsen 4:62a6681510d6 150 tickR++;
kimnielsen 4:62a6681510d6 151 }
kimnielsen 4:62a6681510d6 152 float Dleft() // DISTANCE FOR LEFT WHEEL
kimnielsen 4:62a6681510d6 153 {
kimnielsen 4:62a6681510d6 154 return 2*PI*R*tickL/N;
kimnielsen 4:62a6681510d6 155 }
kimnielsen 4:62a6681510d6 156
kimnielsen 4:62a6681510d6 157 float Dright() // DISTANCE FOR RIGHT WHEEL
kimnielsen 4:62a6681510d6 158 {
kimnielsen 4:62a6681510d6 159 return 2*PI*R*tickR/N;
kimnielsen 4:62a6681510d6 160 }
kimnielsen 4:62a6681510d6 161
kimnielsen 4:62a6681510d6 162 float Dcenter() // DISTANCE FOR CENTER
kimnielsen 4:62a6681510d6 163 {
kimnielsen 4:62a6681510d6 164 return (Dleft()+Dright())/2;
kimnielsen 4:62a6681510d6 165 }
kimnielsen 4:62a6681510d6 166
kimnielsen 4:62a6681510d6 167 float DeltaTHETA() //UPDATE ON ANGLE
kimnielsen 4:62a6681510d6 168 {
kimnielsen 4:62a6681510d6 169 return (Dright()-Dleft())/2*L;
kimnielsen 4:62a6681510d6 170 }
kimnielsen 4:62a6681510d6 171
kimnielsen 4:62a6681510d6 172 // X POSITIONS
kimnielsen 4:62a6681510d6 173 float DeltaX_1() // 1. X POSITION
kimnielsen 4:62a6681510d6 174 {
kimnielsen 4:62a6681510d6 175 return Dcenter()*cos(THETA[0]+DeltaTHETA()/2);
kimnielsen 4:62a6681510d6 176 }
kimnielsen 4:62a6681510d6 177
kimnielsen 4:62a6681510d6 178 // Y POSITIONS
kimnielsen 4:62a6681510d6 179 float DeltaY_1() //1. Y POSITION
kimnielsen 4:62a6681510d6 180 {
kimnielsen 4:62a6681510d6 181 return Dcenter()*sin(THETA[0]+DeltaTHETA()/2);
kimnielsen 4:62a6681510d6 182 }
kimnielsen 4:62a6681510d6 183 // DISTANCES TRAVELLED
kimnielsen 4:62a6681510d6 184 float Dtravel_1() // 1. DISTANCE
kimnielsen 4:62a6681510d6 185 {
kimnielsen 4:62a6681510d6 186 return sqrt(pow(DeltaX_1(), 2) + pow(DeltaY_1(), 2));
kimnielsen 4:62a6681510d6 187 }
kimnielsen 4:62a6681510d6 188
kimnielsen 4:62a6681510d6 189 // PID REGULATOR
kimnielsen 4:62a6681510d6 190 void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CONSIDERATION
kimnielsen 4:62a6681510d6 191 {
kimnielsen 4:62a6681510d6 192 double e = 0; // angle error
kimnielsen 4:62a6681510d6 193 double THETA_D1 = atan((yPosition[1]-yPosition[0])/(xPosition[1]-xPosition[0])); // THETA DESIRED CALCULATED
kimnielsen 4:62a6681510d6 194 double output = 0;
kimnielsen 4:62a6681510d6 195 double derivative = 0;
kimnielsen 4:62a6681510d6 196 double proportional = 0;
kimnielsen 4:62a6681510d6 197 double integral = 0;
kimnielsen 4:62a6681510d6 198 double e_old = 0;
kimnielsen 4:62a6681510d6 199
kimnielsen 4:62a6681510d6 200 while (Dtravel_1() <= DISTANCES[0]) // INDSTIL TIL RIGTIGE PUNKTER!!! SKER I ET PARALLELT TIDSFORLØB MED KØRSEL AF ROBOT TIL PUNKTER
kimnielsen 4:62a6681510d6 201 {
kimnielsen 4:62a6681510d6 202 if(THETA_D1 < -PI) // EVALUATES IF THE ANGLE IS LESS THAN -3.14
kimnielsen 4:62a6681510d6 203 {
kimnielsen 4:62a6681510d6 204 THETA_D1 = -PI;
kimnielsen 4:62a6681510d6 205 }
kimnielsen 4:62a6681510d6 206
kimnielsen 4:62a6681510d6 207 else if(THETA_D1 > PI) // EVALUATES IF THE ANGLE IS MORE THAN 3.14
kimnielsen 4:62a6681510d6 208 {
kimnielsen 4:62a6681510d6 209 THETA_D1 = PI;
kimnielsen 4:62a6681510d6 210 }
kimnielsen 4:62a6681510d6 211
kimnielsen 4:62a6681510d6 212 e = THETA_D1 - THETA[0]; // ERROR VALUE
kimnielsen 4:62a6681510d6 213
kimnielsen 4:62a6681510d6 214 // Compute the proportional
kimnielsen 4:62a6681510d6 215 proportional = e; // get the error
kimnielsen 4:62a6681510d6 216
kimnielsen 4:62a6681510d6 217 // Compute the integral
kimnielsen 4:62a6681510d6 218 integral += proportional;
kimnielsen 4:62a6681510d6 219
kimnielsen 4:62a6681510d6 220 // Compute the derivative
kimnielsen 4:62a6681510d6 221 derivative = e - e_old;
kimnielsen 4:62a6681510d6 222
kimnielsen 4:62a6681510d6 223 // Remember the last error.
kimnielsen 4:62a6681510d6 224 e_old = e;
kimnielsen 4:62a6681510d6 225
kimnielsen 4:62a6681510d6 226 // Compute the output
kimnielsen 4:62a6681510d6 227 output = (proportional * (P_TERM)) + (integral * (I_TERM))
kimnielsen 4:62a6681510d6 228 + (derivative * (D_TERM));
kimnielsen 4:62a6681510d6 229
kimnielsen 4:62a6681510d6 230 // Compute new speeds of both wheels
kimnielsen 4:62a6681510d6 231 right = speed - output;//if power is 0, no error and same speed on wheels
kimnielsen 4:62a6681510d6 232 left = speed + output;
kimnielsen 4:62a6681510d6 233
kimnielsen 4:62a6681510d6 234 // limit checks
kimnielsen 4:62a6681510d6 235 if (right < MIN)
kimnielsen 4:62a6681510d6 236 right = MIN;
kimnielsen 4:62a6681510d6 237 else if (right > MAX)
kimnielsen 4:62a6681510d6 238 right = MAX;
kimnielsen 4:62a6681510d6 239
kimnielsen 4:62a6681510d6 240 if (left < MIN)
kimnielsen 4:62a6681510d6 241 left = MIN;
kimnielsen 4:62a6681510d6 242 else if (left > MAX)
kimnielsen 4:62a6681510d6 243 left = MAX;
kimnielsen 4:62a6681510d6 244
kimnielsen 4:62a6681510d6 245 robot.FW(right,left); // set new positions
kimnielsen 4:62a6681510d6 246 printf("\n\r Dcenter = %3.2f tickL= %4d tickR = %4d left %3.2f right %3.2f",
kimnielsen 4:62a6681510d6 247 Dcenter(),tickL,tickR,left,right);
kimnielsen 4:62a6681510d6 248
kimnielsen 4:62a6681510d6 249 printf("\n\r Error: %.2lf", e); // HUSK DETTE!
kimnielsen 4:62a6681510d6 250
kimnielsen 4:62a6681510d6 251 wait_ms(20); // should be adjusted to your context. Give the motor time
kimnielsen 4:62a6681510d6 252 // to do something before you react
kimnielsen 4:62a6681510d6 253 }
kimnielsen 4:62a6681510d6 254 t.stop(); // stop timer
kimnielsen 4:62a6681510d6 255 }
kimnielsen 4:62a6681510d6 256 #endif