Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

PID.cpp@4:369caebdf5dc, 2016-03-09 (annotated)

Committer:
mpanetta
Date:
Wed Mar 09 17:40:26 2016 +0000
Revision:
4:369caebdf5dc
Parent:
3:aa1a77b080eb 
Making Public

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mpanetta 0:9ac4a91b71fa 1 #include "PID.h"
mpanetta 0:9ac4a91b71fa 2
mpanetta 0:9ac4a91b71fa 3 void
mpanetta 0:9ac4a91b71fa 4 PID::setP(float p)
mpanetta 0:9ac4a91b71fa 5 {
mpanetta 0:9ac4a91b71fa 6 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 0:9ac4a91b71fa 7 k_p = p;
mpanetta 0:9ac4a91b71fa 8 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 0:9ac4a91b71fa 9 }
mpanetta 0:9ac4a91b71fa 10
mpanetta 0:9ac4a91b71fa 11 void
mpanetta 0:9ac4a91b71fa 12 PID::setI(float i)
mpanetta 0:9ac4a91b71fa 13 {
mpanetta 0:9ac4a91b71fa 14 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 0:9ac4a91b71fa 15 k_i = i;
mpanetta 0:9ac4a91b71fa 16 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 0:9ac4a91b71fa 17 }
mpanetta 0:9ac4a91b71fa 18
mpanetta 0:9ac4a91b71fa 19 void
mpanetta 0:9ac4a91b71fa 20 PID::setD(float d)
mpanetta 0:9ac4a91b71fa 21 {
mpanetta 0:9ac4a91b71fa 22 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 0:9ac4a91b71fa 23 k_d = d;
mpanetta 0:9ac4a91b71fa 24 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 0:9ac4a91b71fa 25 }
mpanetta 0:9ac4a91b71fa 26
mpanetta 0:9ac4a91b71fa 27 void
mpanetta 0:9ac4a91b71fa 28 PID::setClip(float clip)
mpanetta 0:9ac4a91b71fa 29 {
mpanetta 0:9ac4a91b71fa 30 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 0:9ac4a91b71fa 31 this->clip = clip;
mpanetta 0:9ac4a91b71fa 32 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 0:9ac4a91b71fa 33 }
mpanetta 0:9ac4a91b71fa 34
mpanetta 0:9ac4a91b71fa 35 void
mpanetta 0:9ac4a91b71fa 36 PID::setPV(float pv) // Set process variable
mpanetta 0:9ac4a91b71fa 37 {
mpanetta 0:9ac4a91b71fa 38 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 0:9ac4a91b71fa 39 error = pv;
mpanetta 0:9ac4a91b71fa 40 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 0:9ac4a91b71fa 41
mpanetta 0:9ac4a91b71fa 42 }
mpanetta 0:9ac4a91b71fa 43
mpanetta 0:9ac4a91b71fa 44 void
mpanetta 0:9ac4a91b71fa 45 PID::dumpDebug(Serial *debug)
mpanetta 0:9ac4a91b71fa 46 {
mpanetta 0:9ac4a91b71fa 47 if (debug != NULL)
mpanetta 0:9ac4a91b71fa 48 {
mpanetta 0:9ac4a91b71fa 49 debug->printf("PID Debug: v_p = %3.2f\r\n", v_p);
mpanetta 0:9ac4a91b71fa 50 debug->printf("PID Debug: v_i = %3.2f\r\n", v_i);
mpanetta 0:9ac4a91b71fa 51 debug->printf("PID Debug: v_d = %3.2f\r\n", v_d);
mpanetta 0:9ac4a91b71fa 52 debug->printf("PID Debug: k_p = %3.2f\r\n", k_p);
mpanetta 0:9ac4a91b71fa 53 debug->printf("PID Debug: k_i = %3.2f\r\n", k_i);
mpanetta 0:9ac4a91b71fa 54 debug->printf("PID Debug: k_d = %3.2f\r\n", k_d);
mpanetta 0:9ac4a91b71fa 55 debug->printf("PID Debug: clip = %3.2f\r\n", clip);
mpanetta 0:9ac4a91b71fa 56 debug->printf("PID Debug: error = %3.2f\r\n", error);
mpanetta 0:9ac4a91b71fa 57 debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
mpanetta 0:9ac4a91b71fa 58 debug->printf("PID Debug: output = %3.2f\r\n", output);
mpanetta 0:9ac4a91b71fa 59 }
mpanetta 0:9ac4a91b71fa 60 }
mpanetta 0:9ac4a91b71fa 61
mpanetta 0:9ac4a91b71fa 62 float
mpanetta 0:9ac4a91b71fa 63 PID::run(float in)
mpanetta 0:9ac4a91b71fa 64 {
mpanetta 0:9ac4a91b71fa 65 if (debug != NULL)
mpanetta 0:9ac4a91b71fa 66 debug->printf("PID Debug: in = %3.2f\r\n", in);
mpanetta 0:9ac4a91b71fa 67 error = in;
mpanetta 0:9ac4a91b71fa 68
mpanetta 0:9ac4a91b71fa 69 v_p = error;
mpanetta 3:aa1a77b080eb 70
mpanetta 3:aa1a77b080eb 71 if (k_i != 0)
mpanetta 3:aa1a77b080eb 72 v_i += v_p;
mpanetta 3:aa1a77b080eb 73 else
mpanetta 3:aa1a77b080eb 74 v_i = 0;
mpanetta 3:aa1a77b080eb 75
mpanetta 0:9ac4a91b71fa 76 v_d = error - prev_error;
mpanetta 0:9ac4a91b71fa 77
mpanetta 0:9ac4a91b71fa 78 if (v_i > clip)
mpanetta 0:9ac4a91b71fa 79 v_i = clip;
mpanetta 3:aa1a77b080eb 80 if (v_i < -clip)
mpanetta 0:9ac4a91b71fa 81 v_i = -clip;
mpanetta 3:aa1a77b080eb 82
mpanetta 0:9ac4a91b71fa 83 if (v_i != v_i) // NAN check...
mpanetta 0:9ac4a91b71fa 84 v_i = 0;
mpanetta 0:9ac4a91b71fa 85
mpanetta 0:9ac4a91b71fa 86 if (debug != NULL)
mpanetta 0:9ac4a91b71fa 87 {
mpanetta 0:9ac4a91b71fa 88 debug->printf("PID Debug: v_p = %3.2f\r\n", v_p);
mpanetta 0:9ac4a91b71fa 89 debug->printf("PID Debug: v_i = %3.2f\r\n", v_i);
mpanetta 0:9ac4a91b71fa 90 debug->printf("PID Debug: v_d = %3.2f\r\n", v_d);
mpanetta 0:9ac4a91b71fa 91 debug->printf("PID Debug: k_p = %3.2f\r\n", k_p);
mpanetta 0:9ac4a91b71fa 92 debug->printf("PID Debug: k_i = %3.2f\r\n", k_i);
mpanetta 0:9ac4a91b71fa 93 debug->printf("PID Debug: k_d = %3.2f\r\n", k_d);
mpanetta 0:9ac4a91b71fa 94 debug->printf("PID Debug: clip = %3.2f\r\n", clip);
mpanetta 0:9ac4a91b71fa 95 }
mpanetta 0:9ac4a91b71fa 96
mpanetta 0:9ac4a91b71fa 97 output = (v_p * k_p) + (v_i * k_i) + (v_d * k_d);
mpanetta 0:9ac4a91b71fa 98
mpanetta 0:9ac4a91b71fa 99 if (debug != NULL)
mpanetta 0:9ac4a91b71fa 100 {
mpanetta 0:9ac4a91b71fa 101 debug->printf("PID Debug: error = %3.2f\r\n", error);
mpanetta 0:9ac4a91b71fa 102 debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
mpanetta 0:9ac4a91b71fa 103 debug->printf("PID Debug: output = %3.2f\r\n", output);
mpanetta 0:9ac4a91b71fa 104 }
mpanetta 0:9ac4a91b71fa 105
mpanetta 0:9ac4a91b71fa 106 prev_error = error;
mpanetta 0:9ac4a91b71fa 107
mpanetta 0:9ac4a91b71fa 108 return output;
mpanetta 0:9ac4a91b71fa 109 }