E2016-S1-Team5 / Mbed 2 deprecated EndeligKildekode

Endelig kildekode med rettelser.

Dependencies:   PID mbed

Fork of Team5robotkode by E2016-S1-Team5

odometry.h@9:f14532fd7a02, 2016-12-15 (annotated)

Committer:
kimnielsen
Date:
Thu Dec 15 13:50:26 2016 +0000
Revision:
9:f14532fd7a02
Parent:
8:5ca76759a67e 
Endelig kildekode med rettelser.

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 9:f14532fd7a02 7 /////////////////////////////////////////////////////////////////////////////
kimnielsen 9:f14532fd7a02 8 // ODOMETRY VALUES //
kimnielsen 9:f14532fd7a02 9 /////////////////////////////////////////////////////////////////////////////
kimnielsen 4:62a6681510d6 10 #define N 20 // ticks on wheel
kimnielsen 8:5ca76759a67e 11 #define R 33.5 // radius = 33.5 mm on wheels
kimnielsen 4:62a6681510d6 12 #define L 133 // 133 mm distance between wheels
kimnielsen 4:62a6681510d6 13 #define PI 3.141592
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 9:f14532fd7a02 17 /////////////////////////////////////////////////////////////////////////////
kimnielsen 9:f14532fd7a02 18 // PID VALUES //
kimnielsen 9:f14532fd7a02 19 /////////////////////////////////////////////////////////////////////////////
kimnielsen 9:f14532fd7a02 20 #define P_TERM 0.13
kimnielsen 7:a852e63cac3d 21 #define I_TERM 0
kimnielsen 4:62a6681510d6 22 #define D_TERM 0
kimnielsen 4:62a6681510d6 23
kimnielsen 9:f14532fd7a02 24 /////////////////////////////////////////////////////////////////////////////
kimnielsen 9:f14532fd7a02 25 // GLOBAL DEFINITIONS //
kimnielsen 9:f14532fd7a02 26 /////////////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 27 double right,left;
kimnielsen 7:a852e63cac3d 28 double result;
kimnielsen 8:5ca76759a67e 29 double speed=0.5;
kimnielsen 9:f14532fd7a02 30 double distance = 4500;
kimnielsen 9:f14532fd7a02 31 /////////////////////////////////////////////////////////////////////////////
kimnielsen 9:f14532fd7a02 32 // ANALOG POWER //
kimnielsen 9:f14532fd7a02 33 /////////////////////////////////////////////////////////////////////////////
kimnielsen 4:62a6681510d6 34 AnalogIn ain(PC_4);
kimnielsen 4:62a6681510d6 35 DigitalOut dout(PB_13);
kimnielsen 4:62a6681510d6 36 DigitalOut greenout(PB_12);
kimnielsen 9:f14532fd7a02 37 int stop=0; //DigitalOut DEFINITION OF RED LED!!
kimnielsen 7:a852e63cac3d 38 DigitalOut LedFlash(PA_3); // RED LED WILL FLASH IF ROBOT STOPS
kimnielsen 4:62a6681510d6 39
kimnielsen 7:a852e63cac3d 40 /////////////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 41 // TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE //
kimnielsen 7:a852e63cac3d 42 /////////////////////////////////////////////////////////////////////////////
kimnielsen 4:62a6681510d6 43 Timer t; // DEFINE A TIMER T
kimnielsen 7:a852e63cac3d 44 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 4:62a6681510d6 45 // connection
kimnielsen 9:f14532fd7a02 46 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left
kimnielsen 9:f14532fd7a02 47 InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right
kimnielsen 9:f14532fd7a02 48 Wheel robot(PB_2, PB_1, PB_15, PB_14);
kimnielsen 4:62a6681510d6 49 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
kimnielsen 4:62a6681510d6 50
kimnielsen 7:a852e63cac3d 51 // GETTING INFO FROM SENSORS
kimnielsen 4:62a6681510d6 52 void read_analog() // comes here every second in this case
kimnielsen 4:62a6681510d6 53 // could be flagged with global variables like "stop"
kimnielsen 4:62a6681510d6 54 {
kimnielsen 4:62a6681510d6 55 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 4:62a6681510d6 56 dout = 1;
kimnielsen 7:a852e63cac3d 57 stop=0;
kimnielsen 4:62a6681510d6 58 } else {
kimnielsen 4:62a6681510d6 59 dout = not dout;
kimnielsen 4:62a6681510d6 60 stop=1; // flash red led
kimnielsen 4:62a6681510d6 61 }
kimnielsen 4:62a6681510d6 62 if (ain==1.0f) greenout = 1; // full power
kimnielsen 4:62a6681510d6 63 else greenout = 0;
kimnielsen 4:62a6681510d6 64 }
kimnielsen 4:62a6681510d6 65 // INIT YOUR PARAMETERS
kimnielsen 8:5ca76759a67e 66 void init()
kimnielsen 4:62a6681510d6 67 {
kimnielsen 4:62a6681510d6 68 tickL=0;
kimnielsen 4:62a6681510d6 69 tickR=0;
kimnielsen 4:62a6681510d6 70 }
kimnielsen 4:62a6681510d6 71 // SENSOR INFO FROM TACO SENSORS
kimnielsen 4:62a6681510d6 72 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 73 {
kimnielsen 4:62a6681510d6 74 tickL++;
kimnielsen 4:62a6681510d6 75 }
kimnielsen 7:a852e63cac3d 76
kimnielsen 4:62a6681510d6 77 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
kimnielsen 4:62a6681510d6 78 {
kimnielsen 4:62a6681510d6 79 tickR++;
kimnielsen 4:62a6681510d6 80 }
kimnielsen 7:a852e63cac3d 81
kimnielsen 4:62a6681510d6 82 float Dleft() // DISTANCE FOR LEFT WHEEL
kimnielsen 4:62a6681510d6 83 {
kimnielsen 8:5ca76759a67e 84 return (2*PI*R*tickL)/N;
kimnielsen 4:62a6681510d6 85 }
kimnielsen 4:62a6681510d6 86
kimnielsen 4:62a6681510d6 87 float Dright() // DISTANCE FOR RIGHT WHEEL
kimnielsen 4:62a6681510d6 88 {
kimnielsen 8:5ca76759a67e 89 return (2*PI*R*tickR)/N;
kimnielsen 4:62a6681510d6 90 }
kimnielsen 4:62a6681510d6 91
kimnielsen 4:62a6681510d6 92 float Dcenter() // DISTANCE FOR CENTER
kimnielsen 4:62a6681510d6 93 {
kimnielsen 4:62a6681510d6 94 return (Dleft()+Dright())/2;
kimnielsen 4:62a6681510d6 95 }
kimnielsen 9:f14532fd7a02 96
kimnielsen 7:a852e63cac3d 97 /////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 98 // PID REGULATOR //
kimnielsen 7:a852e63cac3d 99 /////////////////////////////////////////////////////////////////////
kimnielsen 7:a852e63cac3d 100 void get_to_goal ( )
kimnielsen 4:62a6681510d6 101 {
kimnielsen 7:a852e63cac3d 102 double e = 0;
kimnielsen 4:62a6681510d6 103 double output = 0;
kimnielsen 4:62a6681510d6 104 double derivative = 0;
kimnielsen 4:62a6681510d6 105 double proportional = 0;
kimnielsen 4:62a6681510d6 106 double integral = 0;
kimnielsen 7:a852e63cac3d 107 double e_old = 0;
kimnielsen 4:62a6681510d6 108
kimnielsen 9:f14532fd7a02 109 while (Dcenter() <= distance)
kimnielsen 9:f14532fd7a02 110 {
kimnielsen 9:f14532fd7a02 111 right = speed;
kimnielsen 9:f14532fd7a02 112 left = speed;
kimnielsen 9:f14532fd7a02 113 e = right;
kimnielsen 9:f14532fd7a02 114
kimnielsen 7:a852e63cac3d 115 //PID calculation
kimnielsen 7:a852e63cac3d 116 proportional = e; // GETTING THE ERROR VALUE
kimnielsen 7:a852e63cac3d 117 integral += proportional; // THE ERROR NEVER GETS TOO LOW
kimnielsen 4:62a6681510d6 118 derivative = e - e_old;
kimnielsen 4:62a6681510d6 119 e_old = e;
kimnielsen 7:a852e63cac3d 120
kimnielsen 7:a852e63cac3d 121 // COMPUTING THE OUTPUT SIGNAL
kimnielsen 7:a852e63cac3d 122 output = (proportional * (P_TERM)) + (integral * (I_TERM))+
kimnielsen 7:a852e63cac3d 123 (derivative * (D_TERM));
kimnielsen 7:a852e63cac3d 124
kimnielsen 7:a852e63cac3d 125 // COMPUTING NEW SPEEDS ON WHEELS
kimnielsen 9:f14532fd7a02 126 right = speed - output;
kimnielsen 9:f14532fd7a02 127 left = speed + output*0.15;
kimnielsen 7:a852e63cac3d 128
kimnielsen 9:f14532fd7a02 129 robot.FW(right,left);
kimnielsen 9:f14532fd7a02 130 printf("tickR: %d rightSpeed: %.2lf tickL: %d leftSpeed: %.2lf",
kimnielsen 9:f14532fd7a02 131 tickR, right, tickL, left);
kimnielsen 8:5ca76759a67e 132 wait_ms(10); // should be adjusted to your context. Give the motor time
kimnielsen 4:62a6681510d6 133 // to do something before you react
kimnielsen 4:62a6681510d6 134 }
kimnielsen 4:62a6681510d6 135 t.stop(); // stop timer
kimnielsen 8:5ca76759a67e 136 }
kimnielsen 8:5ca76759a67e 137 #endif