KRAI 2017 / Mbed 2 deprecated prototype_encoder_launcher_V1

tuning pid

Dependencies:   Motor kalman mbed millis

Fork of prototype_encoder_launcher by KRAI 2017

main.cpp@1:4200f28040d3, 2017-02-09 (annotated)

Committer:
Najib_irvani
Date:
Thu Feb 09 13:46:48 2017 +0000
Revision:
1:4200f28040d3
Parent:
0:bc9c90c7b357 
tuning pid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Najib_irvani 0:bc9c90c7b357 1 #include "mbed.h"
Najib_irvani 0:bc9c90c7b357 2 #include "encoderKRAI.h"
Najib_irvani 0:bc9c90c7b357 3 #include "Motor.h"
Najib_irvani 0:bc9c90c7b357 4 #include "millis.h"
Najib_irvani 0:bc9c90c7b357 5
Najib_irvani 1:4200f28040d3 6 double speed, maxSpeed = 0.8, minSpeed = -0.8;
Najib_irvani 1:4200f28040d3 7 double kp=0.167, kd=0.167, ki=0.000167;
Najib_irvani 0:bc9c90c7b357 8 double p,i,d;
Najib_irvani 0:bc9c90c7b357 9 double last_error = 0, current_error, sum_error = 0;
Najib_irvani 1:4200f28040d3 10 double rpm, target_rpm = 2000.0;
Najib_irvani 0:bc9c90c7b357 11
Najib_irvani 0:bc9c90c7b357 12 encoderKRAI encoder( PB_14, PB_13, 14, encoderKRAI::X2_ENCODING);
Najib_irvani 0:bc9c90c7b357 13 Serial pc(USBTX,USBRX);
Najib_irvani 1:4200f28040d3 14 Motor motor(PA_8,PC_2,PC_1);
Najib_irvani 0:bc9c90c7b357 15
Najib_irvani 0:bc9c90c7b357 16 unsigned long int previousMillis = 0;
Najib_irvani 0:bc9c90c7b357 17 unsigned long int currentMillis;
Najib_irvani 0:bc9c90c7b357 18
Najib_irvani 0:bc9c90c7b357 19 void init_speed (){
Najib_irvani 0:bc9c90c7b357 20 if (speed>maxSpeed){
Najib_irvani 0:bc9c90c7b357 21 speed = maxSpeed;
Najib_irvani 0:bc9c90c7b357 22 }
Najib_irvani 0:bc9c90c7b357 23
Najib_irvani 0:bc9c90c7b357 24 if (speed<minSpeed){
Najib_irvani 0:bc9c90c7b357 25 speed = minSpeed;
Najib_irvani 0:bc9c90c7b357 26 }
Najib_irvani 0:bc9c90c7b357 27 }
Najib_irvani 0:bc9c90c7b357 28
Najib_irvani 0:bc9c90c7b357 29
Najib_irvani 0:bc9c90c7b357 30
Najib_irvani 0:bc9c90c7b357 31
Najib_irvani 0:bc9c90c7b357 32 int main() {
Najib_irvani 0:bc9c90c7b357 33
Najib_irvani 0:bc9c90c7b357 34 startMillis();
Najib_irvani 0:bc9c90c7b357 35
Najib_irvani 0:bc9c90c7b357 36
Najib_irvani 0:bc9c90c7b357 37 while(1) {
Najib_irvani 1:4200f28040d3 38
Najib_irvani 0:bc9c90c7b357 39 currentMillis = millis();
Najib_irvani 0:bc9c90c7b357 40
Najib_irvani 0:bc9c90c7b357 41
Najib_irvani 0:bc9c90c7b357 42 if (currentMillis-previousMillis>=50)
Najib_irvani 0:bc9c90c7b357 43 {
Najib_irvani 0:bc9c90c7b357 44 rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0);
Najib_irvani 1:4200f28040d3 45
Najib_irvani 1:4200f28040d3 46 current_error = target_rpm - rpm;
Najib_irvani 1:4200f28040d3 47 sum_error = sum_error + current_error;
Najib_irvani 1:4200f28040d3 48 p = current_error*kp;
Najib_irvani 1:4200f28040d3 49 d = (current_error-last_error)*kd/50.0;
Najib_irvani 1:4200f28040d3 50 i = sum_error*ki*50.0;
Najib_irvani 1:4200f28040d3 51
Najib_irvani 1:4200f28040d3 52 speed = p + d + i;
Najib_irvani 1:4200f28040d3 53 init_speed();
Najib_irvani 1:4200f28040d3 54 motor.speed(speed);
Najib_irvani 1:4200f28040d3 55 last_error = current_error;
Najib_irvani 1:4200f28040d3 56
Najib_irvani 0:bc9c90c7b357 57 encoder.reset();
Najib_irvani 0:bc9c90c7b357 58 pc.printf("%.04lf\n",rpm);
Najib_irvani 0:bc9c90c7b357 59
Najib_irvani 1:4200f28040d3 60 previousMillis = currentMillis;
Najib_irvani 1:4200f28040d3 61
Najib_irvani 0:bc9c90c7b357 62 }
Najib_irvani 1:4200f28040d3 63
Najib_irvani 1:4200f28040d3 64
Najib_irvani 0:bc9c90c7b357 65
Najib_irvani 0:bc9c90c7b357 66 }
Najib_irvani 0:bc9c90c7b357 67 }