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@0:bc9c90c7b357, 2017-02-08 (annotated)

Committer:
Najib_irvani
Date:
Wed Feb 08 13:55:01 2017 +0000
Revision:
0:bc9c90c7b357
Child:
1:4200f28040d3
prototype  encoder launcher

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 0:bc9c90c7b357 6 double speed, maxSpeed = 0.8, minSpeed = -0.8, last_speed = 0.0;
Najib_irvani 0:bc9c90c7b357 7 double kp=0.0005, kd=0.005, ki=0.000005;
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 0:bc9c90c7b357 10 double rpm, target_rpm = 1500.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 0:bc9c90c7b357 14 Motor motor(PA_8,PC_1,PC_2);
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 0:bc9c90c7b357 38 current_error = target_rpm - rpm;
Najib_irvani 0:bc9c90c7b357 39 sum_error += current_error;
Najib_irvani 0:bc9c90c7b357 40 p = current_error*kp;
Najib_irvani 0:bc9c90c7b357 41 d = (current_error-last_error)*kd;
Najib_irvani 0:bc9c90c7b357 42 i = sum_error*ki;
Najib_irvani 0:bc9c90c7b357 43
Najib_irvani 0:bc9c90c7b357 44 speed = last_speed + p + d;
Najib_irvani 0:bc9c90c7b357 45 init_speed();
Najib_irvani 0:bc9c90c7b357 46 motor.speed(speed);
Najib_irvani 0:bc9c90c7b357 47 currentMillis = millis();
Najib_irvani 0:bc9c90c7b357 48
Najib_irvani 0:bc9c90c7b357 49
Najib_irvani 0:bc9c90c7b357 50 if (currentMillis-previousMillis>=50)
Najib_irvani 0:bc9c90c7b357 51 {
Najib_irvani 0:bc9c90c7b357 52 rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0);
Najib_irvani 0:bc9c90c7b357 53 previousMillis = currentMillis;
Najib_irvani 0:bc9c90c7b357 54 encoder.reset();
Najib_irvani 0:bc9c90c7b357 55 pc.printf("%.04lf\n",rpm);
Najib_irvani 0:bc9c90c7b357 56
Najib_irvani 0:bc9c90c7b357 57 }
Najib_irvani 0:bc9c90c7b357 58 //last_speed = speed;
Najib_irvani 0:bc9c90c7b357 59 last_error = current_error;
Najib_irvani 0:bc9c90c7b357 60
Najib_irvani 0:bc9c90c7b357 61 }
Najib_irvani 0:bc9c90c7b357 62 }