E2016-S1-Team5 / Mbed 2 deprecated Team5

Kildekode til robot

Dependencies:   PID mbed

Fork of Robotkode by Kim Nielsen

odometry.h@7:a852e63cac3d, 2016-12-09 (annotated)

Committer:
kimnielsen
Date:
Fri Dec 09 12:32:21 2016 +0000
Revision:
7:a852e63cac3d
Parent:
5:cf033e9d4468 
Kildekode til robot

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