Biorobotics_group_2 / Mbed 2 deprecated Motor_PID_set_parameters

Sets the Kp, Ki and Kd values of the PID controller for the encoder motors

Dependencies:   FastPWM HIDScope_motor_ff MODSERIAL QEI mbed

Fork of Encoder by Biorobotics_group_2

main.cpp@6:ed11342ab079, 2016-10-07 (annotated)

Committer:
sjoerdbarts
Date:
Fri Oct 07 12:26:22 2016 +0000
Revision:
6:ed11342ab079
Parent:
5:36788b154e25
Child:
7:e7aa4f10d1fb
Calculate speed added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sjoerdbarts 0:1883abafaa19 1 #include "mbed.h"
sjoerdbarts 2:5fce9d33997f 2 #include "HIDScope.h"
sjoerdbarts 5:36788b154e25 3 #include "QEI.h"
sjoerdbarts 3:ce0f979f15fb 4 #define SERIAL_BAUD 115200 // baud rate for serial communication
sjoerdbarts 3:ce0f979f15fb 5
sjoerdbarts 5:36788b154e25 6 Serial pc(USBTX,USBRX);
sjoerdbarts 3:ce0f979f15fb 7
sjoerdbarts 5:36788b154e25 8 int countsCW = 0;
sjoerdbarts 5:36788b154e25 9 int countsCCW = 0;
sjoerdbarts 5:36788b154e25 10 int net_counts = 0;
sjoerdbarts 5:36788b154e25 11 float degrees = 0.0;
sjoerdbarts 6:ed11342ab079 12
sjoerdbarts 6:ed11342ab079 13 volatile float curr_degrees = 0.0;
sjoerdbarts 6:ed11342ab079 14 volatile float prev_degrees = 0.0;
sjoerdbarts 6:ed11342ab079 15 volatile float speed = 0.0; // speed in degrees/s
sjoerdbarts 6:ed11342ab079 16 volatile const float T_CalculateSpeed = 0.1; // 100 Hz
sjoerdbarts 6:ed11342ab079 17
sjoerdbarts 5:36788b154e25 18 const float counts_per_rev = 4200.0;
sjoerdbarts 5:36788b154e25 19 QEI EncoderCW(D12,D13,NC,32);
sjoerdbarts 5:36788b154e25 20 QEI EncoderCCW(D13,D12,NC,32);
sjoerdbarts 0:1883abafaa19 21
sjoerdbarts 5:36788b154e25 22 void PrintDegrees(){
sjoerdbarts 5:36788b154e25 23 pc.printf("\r\n Nett Pulses %i \r\n", net_counts);
sjoerdbarts 5:36788b154e25 24 pc.printf("\r\n Output degrees %f \r\n", degrees);
sjoerdbarts 6:ed11342ab079 25 pc.printf("\r\n Speed %f \r\n",speed);
sjoerdbarts 5:36788b154e25 26 }
sjoerdbarts 6:ed11342ab079 27
sjoerdbarts 6:ed11342ab079 28 void CalculateSpeed() {
sjoerdbarts 6:ed11342ab079 29 curr_degrees = degrees;
sjoerdbarts 6:ed11342ab079 30 speed = (curr_degrees-prev_degrees)/T_CalculateSpeed;
sjoerdbarts 6:ed11342ab079 31 prev_degrees = curr_degrees;
sjoerdbarts 6:ed11342ab079 32 }
sjoerdbarts 5:36788b154e25 33
sjoerdbarts 0:1883abafaa19 34 int main()
sjoerdbarts 0:1883abafaa19 35 {
sjoerdbarts 3:ce0f979f15fb 36 pc.baud(SERIAL_BAUD);
sjoerdbarts 3:ce0f979f15fb 37 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
sjoerdbarts 3:ce0f979f15fb 38
sjoerdbarts 5:36788b154e25 39 // Set ticker for serial communication of counts and degrees
sjoerdbarts 5:36788b154e25 40 Ticker PrintDegreesTicker;
sjoerdbarts 5:36788b154e25 41 PrintDegreesTicker.attach(&PrintDegrees,0.1);
sjoerdbarts 2:5fce9d33997f 42
sjoerdbarts 6:ed11342ab079 43 // Set ticker for speed calculation
sjoerdbarts 6:ed11342ab079 44 Ticker CalculateSpeedTicker;
sjoerdbarts 6:ed11342ab079 45 CalculateSpeedTicker.attach(CalculateSpeed,T_CalculateSpeed);
sjoerdbarts 6:ed11342ab079 46
sjoerdbarts 5:36788b154e25 47 // count the CW and CCW counts and calculate the output degrees
sjoerdbarts 5:36788b154e25 48 while(true){
sjoerdbarts 5:36788b154e25 49 countsCW = EncoderCW.getPulses();
sjoerdbarts 5:36788b154e25 50 countsCCW= EncoderCCW.getPulses();
sjoerdbarts 5:36788b154e25 51 net_counts=countsCW-countsCCW;
sjoerdbarts 5:36788b154e25 52 degrees=(net_counts*360.0)/counts_per_rev;
sjoerdbarts 5:36788b154e25 53
sjoerdbarts 5:36788b154e25 54 }
sjoerdbarts 0:1883abafaa19 55 }