Kim Nielsen / Mbed 2 deprecated Endeligkildekode

Endelig kildekode med rettelser.

Dependencies:   PID mbed

Fork of EndeligKildekode by E2016-S1-Team5

odometry.h@8:5ca76759a67e, 2016-12-12 (annotated)

Committer:
kimnielsen
Date:
Mon Dec 12 13:35:34 2016 +0000
Revision:
8:5ca76759a67e
Parent:
7:a852e63cac3d
Child:
9:f14532fd7a02
hej

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 #include "mbed.h"
kimnielsen 4:62a6681510d6 4 #include "hack_motor.h"
kimnielsen 7:a852e63cac3d 5 #include "math.h"
kimnielsen 4:62a6681510d6 6
kimnielsen 4:62a6681510d6 7 // ODOMETRY VALUES
kimnielsen 4:62a6681510d6 8 #define N 20 // ticks on wheel
kimnielsen 8:5ca76759a67e 9 #define R 33.5 // radius = 33.5 mm on wheels
kimnielsen 4:62a6681510d6 10 #define L 133 // 133 mm distance between wheels
kimnielsen 4:62a6681510d6 11 #define PI 3.141592
kimnielsen 8:5ca76759a67e 12 #define DISTANCE 10000
kimnielsen 8:5ca76759a67e 13 #define TURN 282
kimnielsen 4:62a6681510d6 14 int tickL = 0; // tick on left wheel
kimnielsen 4:62a6681510d6 15 int tickR = 0; // tick on right wheel
kimnielsen 4:62a6681510d6 16
kimnielsen 4:62a6681510d6 17 // PID VALUES
kimnielsen 4:62a6681510d6 18 #define P_TERM 1.0
kimnielsen 7:a852e63cac3d 19 #define I_TERM 0
kimnielsen 4:62a6681510d6 20 #define D_TERM 0
kimnielsen 8:5ca76759a67e 21 #define MIN 0.9
kimnielsen 7:a852e63cac3d 22 #define MAX 1.0
kimnielsen 8:5ca76759a67e 23 #define WheelFactor 0.10
kimnielsen 8:5ca76759a67e 24 #define T_MIN 0.00
kimnielsen 8:5ca76759a67e 25 #define T_MAX 1.00
kimnielsen 4:62a6681510d6 26
kimnielsen 4:62a6681510d6 27 // GLOBAL DEFINITIONS
kimnielsen 7:a852e63cac3d 28 double right,left;
kimnielsen 7:a852e63cac3d 29 double result;
kimnielsen 8:5ca76759a67e 30 double speed=0.5;
kimnielsen 8:5ca76759a67e 31 double speed_turn=1;
kimnielsen 7:a852e63cac3d 32 DigitalOut LED(LED1);
kimnielsen 4:62a6681510d6 33
kimnielsen 4:62a6681510d6 34 // ANALOG POWER
kimnielsen 4:62a6681510d6 35 AnalogIn ain(PC_4);
kimnielsen 4:62a6681510d6 36 DigitalOut dout(PB_13);
kimnielsen 4:62a6681510d6 37 DigitalOut greenout(PB_12);
kimnielsen 4:62a6681510d6 38 int stop=0; //DigitalOut DEFINITION OF RED LED!!
kimnielsen 7:a852e63cac3d 39 DigitalOut LedFlash(PA_3); // RED LED WILL FLASH IF ROBOT STOPS
kimnielsen 4:62a6681510d6 40
kimnielsen 7:a852e63cac3d 41 /////////////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 42 // TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE //
kimnielsen 7:a852e63cac3d 43 /////////////////////////////////////////////////////////////////////////////
kimnielsen 4:62a6681510d6 44 Timer t; // DEFINE A TIMER T
kimnielsen 7:a852e63cac3d 45 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 4:62a6681510d6 46 // connection
kimnielsen 4:62a6681510d6 47 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
kimnielsen 7:a852e63cac3d 48 InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå
kimnielsen 4:62a6681510d6 49 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 50 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
kimnielsen 4:62a6681510d6 51
kimnielsen 7:a852e63cac3d 52 // GETTING INFO FROM SENSORS
kimnielsen 4:62a6681510d6 53 void read_analog() // comes here every second in this case
kimnielsen 4:62a6681510d6 54 // could be flagged with global variables like "stop"
kimnielsen 4:62a6681510d6 55 {
kimnielsen 4:62a6681510d6 56 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 4:62a6681510d6 57 dout = 1;
kimnielsen 7:a852e63cac3d 58 stop=0;
kimnielsen 4:62a6681510d6 59 } else {
kimnielsen 4:62a6681510d6 60 dout = not dout;
kimnielsen 4:62a6681510d6 61 stop=1; // flash red led
kimnielsen 7:a852e63cac3d 62 LED = 1;
kimnielsen 4:62a6681510d6 63 }
kimnielsen 4:62a6681510d6 64 if (ain==1.0f) greenout = 1; // full power
kimnielsen 4:62a6681510d6 65 else greenout = 0;
kimnielsen 4:62a6681510d6 66 }
kimnielsen 4:62a6681510d6 67 // INIT YOUR PARAMETERS
kimnielsen 8:5ca76759a67e 68 void init()
kimnielsen 4:62a6681510d6 69 {
kimnielsen 4:62a6681510d6 70 tickL=0;
kimnielsen 4:62a6681510d6 71 tickR=0;
kimnielsen 4:62a6681510d6 72 }
kimnielsen 4:62a6681510d6 73 // SENSOR INFO FROM TACO SENSORS
kimnielsen 4:62a6681510d6 74 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 75 {
kimnielsen 4:62a6681510d6 76 tickL++;
kimnielsen 4:62a6681510d6 77 }
kimnielsen 7:a852e63cac3d 78
kimnielsen 4:62a6681510d6 79 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 80 {
kimnielsen 4:62a6681510d6 81 tickR++;
kimnielsen 4:62a6681510d6 82 }
kimnielsen 7:a852e63cac3d 83
kimnielsen 4:62a6681510d6 84 float Dleft() // DISTANCE FOR LEFT WHEEL
kimnielsen 4:62a6681510d6 85 {
kimnielsen 8:5ca76759a67e 86 return (2*PI*R*tickL)/N;
kimnielsen 4:62a6681510d6 87 }
kimnielsen 4:62a6681510d6 88
kimnielsen 4:62a6681510d6 89 float Dright() // DISTANCE FOR RIGHT WHEEL
kimnielsen 4:62a6681510d6 90 {
kimnielsen 8:5ca76759a67e 91 return (2*PI*R*tickR)/N;
kimnielsen 4:62a6681510d6 92 }
kimnielsen 4:62a6681510d6 93
kimnielsen 4:62a6681510d6 94 float Dcenter() // DISTANCE FOR CENTER
kimnielsen 4:62a6681510d6 95 {
kimnielsen 4:62a6681510d6 96 return (Dleft()+Dright())/2;
kimnielsen 4:62a6681510d6 97 }
kimnielsen 4:62a6681510d6 98
kimnielsen 7:a852e63cac3d 99 /////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 100 // PID REGULATOR //
kimnielsen 7:a852e63cac3d 101 /////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 102 void get_to_goal ( )
kimnielsen 4:62a6681510d6 103 {
kimnielsen 7:a852e63cac3d 104 double e = 0;
kimnielsen 4:62a6681510d6 105 double output = 0;
kimnielsen 4:62a6681510d6 106 double derivative = 0;
kimnielsen 4:62a6681510d6 107 double proportional = 0;
kimnielsen 4:62a6681510d6 108 double integral = 0;
kimnielsen 7:a852e63cac3d 109 double e_old = 0;
kimnielsen 4:62a6681510d6 110
kimnielsen 7:a852e63cac3d 111 while (Dcenter() <= DISTANCE)
kimnielsen 7:a852e63cac3d 112 {
kimnielsen 7:a852e63cac3d 113 e = tickR - tickL; //error calculation
kimnielsen 7:a852e63cac3d 114 if(e < 0)
kimnielsen 4:62a6681510d6 115 {
kimnielsen 8:5ca76759a67e 116 tickR = tickL;
kimnielsen 7:a852e63cac3d 117 }
kimnielsen 7:a852e63cac3d 118 else if(e > 0)
kimnielsen 7:a852e63cac3d 119 {
kimnielsen 8:5ca76759a67e 120 tickR = tickL;
kimnielsen 4:62a6681510d6 121 }
kimnielsen 4:62a6681510d6 122
kimnielsen 7:a852e63cac3d 123 //PID calculation
kimnielsen 7:a852e63cac3d 124 proportional = e; // GETTING THE ERROR VALUE
kimnielsen 7:a852e63cac3d 125 integral += proportional; // THE ERROR NEVER GETS TOO LOW
kimnielsen 4:62a6681510d6 126 derivative = e - e_old;
kimnielsen 4:62a6681510d6 127 e_old = e;
kimnielsen 7:a852e63cac3d 128
kimnielsen 7:a852e63cac3d 129 // COMPUTING THE OUTPUT SIGNAL
kimnielsen 7:a852e63cac3d 130 output = (proportional * (P_TERM)) + (integral * (I_TERM))+
kimnielsen 7:a852e63cac3d 131 (derivative * (D_TERM));
kimnielsen 7:a852e63cac3d 132
kimnielsen 7:a852e63cac3d 133 // COMPUTING NEW SPEEDS ON WHEELS
kimnielsen 8:5ca76759a67e 134 right = speed + output;//if power is 0, no error->same speed on wheels
kimnielsen 8:5ca76759a67e 135 right = right - WheelFactor*right;
kimnielsen 4:62a6681510d6 136 left = speed + output;
kimnielsen 7:a852e63cac3d 137
kimnielsen 7:a852e63cac3d 138 //CHECK YOUR LIMITS
kimnielsen 7:a852e63cac3d 139 if(right < MIN)
kimnielsen 7:a852e63cac3d 140 {
kimnielsen 8:5ca76759a67e 141 right = MIN- WheelFactor*MIN;
kimnielsen 7:a852e63cac3d 142 }
kimnielsen 7:a852e63cac3d 143 else if(right > MAX)
kimnielsen 7:a852e63cac3d 144 {
kimnielsen 8:5ca76759a67e 145 right = MAX- WheelFactor*MAX;
kimnielsen 7:a852e63cac3d 146 }
kimnielsen 7:a852e63cac3d 147 if(left < MIN)
kimnielsen 7:a852e63cac3d 148 {
kimnielsen 4:62a6681510d6 149 left = MIN;
kimnielsen 7:a852e63cac3d 150 }
kimnielsen 7:a852e63cac3d 151 else if(left > MAX)
kimnielsen 7:a852e63cac3d 152 {
kimnielsen 4:62a6681510d6 153 left = MAX;
kimnielsen 7:a852e63cac3d 154 }
kimnielsen 7:a852e63cac3d 155 printf("\n\r RightSpeed: %.2lf RightTicks: %d LeftSpeed: %.2lf"
kimnielsen 8:5ca76759a67e 156 "LeftTicks: %d Output: %lf", right, tickR, left, tickL, output);
kimnielsen 7:a852e63cac3d 157
kimnielsen 7:a852e63cac3d 158 // set new positions
kimnielsen 7:a852e63cac3d 159 robot.FW(right,left);
kimnielsen 8:5ca76759a67e 160 wait_ms(10); // should be adjusted to your context. Give the motor time
kimnielsen 4:62a6681510d6 161 // to do something before you react
kimnielsen 4:62a6681510d6 162 }
kimnielsen 4:62a6681510d6 163 t.stop(); // stop timer
kimnielsen 8:5ca76759a67e 164 }
kimnielsen 8:5ca76759a67e 165
kimnielsen 8:5ca76759a67e 166 void turn()
kimnielsen 8:5ca76759a67e 167 {
kimnielsen 8:5ca76759a67e 168 double e = 0;
kimnielsen 8:5ca76759a67e 169 double output = 0;
kimnielsen 8:5ca76759a67e 170 double derivative = 0;
kimnielsen 8:5ca76759a67e 171 double proportional = 0;
kimnielsen 8:5ca76759a67e 172 double integral = 0;
kimnielsen 8:5ca76759a67e 173 double e_old = 0;
kimnielsen 8:5ca76759a67e 174
kimnielsen 8:5ca76759a67e 175 while(Dright() <= TURN)
kimnielsen 8:5ca76759a67e 176 {
kimnielsen 8:5ca76759a67e 177 e = tickR - tickL;
kimnielsen 8:5ca76759a67e 178 if(e < 0)
kimnielsen 8:5ca76759a67e 179 {
kimnielsen 8:5ca76759a67e 180 tickR = tickL;
kimnielsen 8:5ca76759a67e 181 }
kimnielsen 8:5ca76759a67e 182 else if(e > 0)
kimnielsen 8:5ca76759a67e 183 {
kimnielsen 8:5ca76759a67e 184 tickR = tickL;
kimnielsen 8:5ca76759a67e 185 }
kimnielsen 8:5ca76759a67e 186 proportional = e;
kimnielsen 8:5ca76759a67e 187 integral += proportional;
kimnielsen 8:5ca76759a67e 188 derivative = e - e_old;
kimnielsen 8:5ca76759a67e 189 e_old = e;
kimnielsen 8:5ca76759a67e 190
kimnielsen 8:5ca76759a67e 191 output = (proportional * (P_TERM)) + (integral * (I_TERM))+
kimnielsen 8:5ca76759a67e 192 (derivative * (D_TERM));
kimnielsen 8:5ca76759a67e 193 right = speed + output;
kimnielsen 8:5ca76759a67e 194 left = 0;
kimnielsen 8:5ca76759a67e 195
kimnielsen 8:5ca76759a67e 196 if(right <= T_MIN)
kimnielsen 8:5ca76759a67e 197 {
kimnielsen 8:5ca76759a67e 198 right = T_MAX;
kimnielsen 8:5ca76759a67e 199 }
kimnielsen 8:5ca76759a67e 200
kimnielsen 8:5ca76759a67e 201 else if(right >= T_MAX)
kimnielsen 8:5ca76759a67e 202 {
kimnielsen 8:5ca76759a67e 203 right = T_MAX;
kimnielsen 8:5ca76759a67e 204 }
kimnielsen 8:5ca76759a67e 205 robot.FW(right, left);
kimnielsen 8:5ca76759a67e 206 wait_ms(10);
kimnielsen 8:5ca76759a67e 207 }
kimnielsen 4:62a6681510d6 208 }
kimnielsen 8:5ca76759a67e 209 #endif