Vera Stakenborg / Mbed 2 deprecated Project_motor

test with PID control to screen on your computer

Dependencies:   Motor_with_encoder MODSERIAL QEI mbed

Fork of Project_motor by Biorobotics Project

main.cpp@8:aff2a7d5861a, 2017-10-13 (annotated)

Committer:
vera1
Date:
Fri Oct 13 12:27:41 2017 +0000
Revision:
8:aff2a7d5861a
Parent:
5:05d09e921b5d 
test for compiling;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vera1 0:bc949f735b8d 1 #include "mbed.h"
vera1 0:bc949f735b8d 2 #include "MODSERIAL.h"
vera1 0:bc949f735b8d 3 #include "encoder.h"
vera1 0:bc949f735b8d 4 // this program can turn the motor clockwise or counterclockwise depending on the value p or n typed into the terminal. with 's' you can stop the motor
vera1 0:bc949f735b8d 5
vera1 0:bc949f735b8d 6 MODSERIAL pc(USBTX,USBRX);
vera1 8:aff2a7d5861a 7 PwmOut motorspeed(D6);
vera1 8:aff2a7d5861a 8 PwmOut motorspeed2(D5);
vera1 8:aff2a7d5861a 9 DigitalOut motorposition2(D4);
vera1 8:aff2a7d5861a 10 DigitalOut motorposition(D7);
vera1 0:bc949f735b8d 11 DigitalOut led1(D8);
vera1 1:e0f6cdefcd6e 12 DigitalOut led2(D11);
vera1 0:bc949f735b8d 13 AnalogIn pot(A1);
vera1 8:aff2a7d5861a 14 AnalogIn pot2(A2);
vera1 2:80177e90c1e6 15 DigitalIn button1(D13);
vera1 2:80177e90c1e6 16 DigitalIn button2(D12);
vera1 0:bc949f735b8d 17 Ticker potmeterreadout;
vera1 1:e0f6cdefcd6e 18 Ticker encoderreadout;
vera1 1:e0f6cdefcd6e 19 Encoder motor1(PTD0,PTC4); // motor 1 is from encoder, otherwise it would be a pain in the ass to change all that code to motorencoder
vera1 0:bc949f735b8d 20
vera1 0:bc949f735b8d 21
vera1 3:4520d0ca4e56 22 float PwmPeriod = 0.0001f;
MarkHeimgartner 5:05d09e921b5d 23
vera1 8:aff2a7d5861a 24 const double M1_TS = 0.01f; // timestep
vera1 8:aff2a7d5861a 25 const double M1_KP = 0.216, M1_KI = 1.8, M1_KD = 0.0; // controller gains for motor 1
vera1 3:4520d0ca4e56 26 double m1_err_int = 0, m1_prev_err = 0; // initiallize errors
vera1 3:4520d0ca4e56 27
vera1 3:4520d0ca4e56 28 // PID controller function
vera1 8:aff2a7d5861a 29 double PI(double e, const double Kp, const double Ki, const double Kd, double Ts, double&e_int){
vera1 8:aff2a7d5861a 30 //double e_der = (e - e_prev)/Ts; // derivative
vera1 8:aff2a7d5861a 31 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
vera1 8:aff2a7d5861a 32 //e_prev = e;
vera1 3:4520d0ca4e56 33 e_int = e_int + Ts*e; // integral
vera1 8:aff2a7d5861a 34 return Kp*e + Ki*e_int; //PI controller
vera1 3:4520d0ca4e56 35 }
vera1 3:4520d0ca4e56 36
vera1 3:4520d0ca4e56 37
vera1 1:e0f6cdefcd6e 38 volatile float position = 0.0;
vera1 0:bc949f735b8d 39 void readpot(){
MarkHeimgartner 4:abdc2d24d0bc 40 position = motor1.getPosition()/4200.00*6.28;
vera1 8:aff2a7d5861a 41 float motorpi = PI(reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int);
vera1 8:aff2a7d5861a 42 pc.printf("PI output = %f, reference = %i\n\r", motorpi, reference);
vera1 8:aff2a7d5861a 43 //motorspeed = motorpi;
vera1 0:bc949f735b8d 44 }
vera1 8:aff2a7d5861a 45
vera1 3:4520d0ca4e56 46
vera1 0:bc949f735b8d 47 int main()
vera1 1:e0f6cdefcd6e 48 {
vera1 8:aff2a7d5861a 49 float reference = 0.0;
vera1 0:bc949f735b8d 50 pc.baud(9600);
vera1 0:bc949f735b8d 51 potmeterreadout.attach(readpot,0.2f);
vera1 8:aff2a7d5861a 52 motorspeed2.period(PwmPeriod);
vera1 0:bc949f735b8d 53 //float motorspeed = 0.0f;
vera1 0:bc949f735b8d 54 while (true) {
vera1 8:aff2a7d5861a 55 reference = 0.0;
vera1 8:aff2a7d5861a 56 wait(2.0f);
vera1 8:aff2a7d5861a 57 reference = 1.0;
vera1 8:aff2a7d5861a 58 wait(5.0f);
vera1 8:aff2a7d5861a 59
vera1 0:bc949f735b8d 60 }
vera1 0:bc949f735b8d 61 }
vera1 0:bc949f735b8d 62