Jasper Gerth / Mbed 2 deprecated P-control

P controller to make the motor follow pot1

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@2:04ad19deeb2c, 2015-09-23 (annotated)

Committer:
Gerth
Date:
Wed Sep 23 14:12:15 2015 +0000
Revision:
2:04ad19deeb2c
Parent:
1:f367ab17bc18
Child:
3:c39510de9443
P-controller almost done;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:3df25d5b2946 1 #include "mbed.h"
Gerth 1:f367ab17bc18 2 #include "MODSERIAL.h"
Gerth 1:f367ab17bc18 3 #include "QEI.h"
Gerth 1:f367ab17bc18 4 #include "HIDScope.h"
Gerth 1:f367ab17bc18 5
Gerth 2:04ad19deeb2c 6 //info uit
Gerth 1:f367ab17bc18 7 HIDScope scope(2);
Gerth 1:f367ab17bc18 8 MODSERIAL pc(USBTX, USBRX);
Gerth 2:04ad19deeb2c 9
Gerth 2:04ad19deeb2c 10 //encoders
Gerth 1:f367ab17bc18 11 QEI encoder1 (D13,D12,NC,32);
Gerth 1:f367ab17bc18 12
Gerth 1:f367ab17bc18 13 //ingangen
Gerth 1:f367ab17bc18 14 AnalogIn pot1(A0);
Gerth 1:f367ab17bc18 15
Gerth 2:04ad19deeb2c 16 //uitgangen
Gerth 2:04ad19deeb2c 17 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW shaft) shaft draait tegen encoder in!
Gerth 2:04ad19deeb2c 18 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 2:04ad19deeb2c 19 //PwmOut motor2_speed_control(D5);
Gerth 2:04ad19deeb2c 20 //DigitalOut motor2_direction(D4);
Gerth 2:04ad19deeb2c 21 const int CW=0; //clockwise
Gerth 2:04ad19deeb2c 22 const int CCW=1; //counterclockwise
Gerth 2:04ad19deeb2c 23
Gerth 1:f367ab17bc18 24 //frequencies
Gerth 2:04ad19deeb2c 25 const float pwm_frequency=1000;
Gerth 1:f367ab17bc18 26 const float hidscope_frequency=100;
Gerth 2:04ad19deeb2c 27 const float p_control_frequency=5;
Gerth 1:f367ab17bc18 28
Gerth 1:f367ab17bc18 29 //tickers
Gerth 1:f367ab17bc18 30 Ticker hidscope_ticker;
Gerth 1:f367ab17bc18 31 Ticker p_control_ticker;
Gerth 0:3df25d5b2946 32
Gerth 1:f367ab17bc18 33 //constants
Gerth 1:f367ab17bc18 34 const int cpr=32;
Gerth 1:f367ab17bc18 35 const float PI=3.1415;
Gerth 1:f367ab17bc18 36 const float counttorad=((2*PI)/cpr);
Gerth 2:04ad19deeb2c 37 const float motor1_p_kp=0.005;
Gerth 1:f367ab17bc18 38
Gerth 1:f367ab17bc18 39 //go flags
Gerth 1:f367ab17bc18 40 volatile bool scopedata_go=false, p_control_go=false;
Gerth 1:f367ab17bc18 41
Gerth 1:f367ab17bc18 42 //acvitator functions
Gerth 1:f367ab17bc18 43
Gerth 1:f367ab17bc18 44 void scopedata_activate()
Gerth 1:f367ab17bc18 45 {
Gerth 1:f367ab17bc18 46 scopedata_go=true;
Gerth 1:f367ab17bc18 47 }
Gerth 2:04ad19deeb2c 48 void p_control_activate()
Gerth 2:04ad19deeb2c 49 {
Gerth 1:f367ab17bc18 50 p_control_go=true;
Gerth 2:04ad19deeb2c 51 }
Gerth 1:f367ab17bc18 52 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 1:f367ab17bc18 53
Gerth 1:f367ab17bc18 54 //scopedata
Gerth 1:f367ab17bc18 55 void scopedata()
Gerth 1:f367ab17bc18 56 {
Gerth 1:f367ab17bc18 57 scope.set(0,pot1.read());
Gerth 1:f367ab17bc18 58 scope.set(1,counttorad*encoder1.getPulses());
Gerth 1:f367ab17bc18 59 scope.send();
Gerth 1:f367ab17bc18 60 }
Gerth 2:04ad19deeb2c 61
Gerth 2:04ad19deeb2c 62 //re usable P controller
Gerth 2:04ad19deeb2c 63 double p_control(error,kp)
Gerth 2:04ad19deeb2c 64 {
Gerth 2:04ad19deeb2c 65 return (error*kp);
Gerth 2:04ad19deeb2c 66 }
Gerth 0:3df25d5b2946 67
Gerth 0:3df25d5b2946 68 int main()
Gerth 0:3df25d5b2946 69 {
Gerth 1:f367ab17bc18 70 //set initial shizzle
Gerth 2:04ad19deeb2c 71
Gerth 1:f367ab17bc18 72
Gerth 2:04ad19deeb2c 73 //tickers
Gerth 1:f367ab17bc18 74 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 1:f367ab17bc18 75 p_control_ticker.attach(&p_control_activate,1.0/p_control_frequency);
Gerth 1:f367ab17bc18 76
Gerth 1:f367ab17bc18 77 while(1) {
Gerth 2:04ad19deeb2c 78 //control motor 1 with a p controller
Gerth 1:f367ab17bc18 79 if (p_control_go==true) {
Gerth 2:04ad19deeb2c 80 float signal_motor1=p_control(pot1.read()-encoder1.getPulses(),motor1_p_kp);
Gerth 2:04ad19deeb2c 81 if (signal_motor1>=0) {//determine CW or CCW rotation
Gerth 2:04ad19deeb2c 82 motor1_direction.write(CW);
Gerth 2:04ad19deeb2c 83 } else {
Gerth 2:04ad19deeb2c 84 motor1_direction.write(CCW);
Gerth 2:04ad19deeb2c 85 }
Gerth 2:04ad19deeb2c 86 if fabs(signal_motor1>=1) { //check if signal is <1
Gerth 2:04ad19deeb2c 87 signal_motor1=1;
Gerth 2:04ad19deeb2c 88 }
Gerth 2:04ad19deeb2c 89 motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
Gerth 1:f367ab17bc18 90 p_control_go=false;
Gerth 1:f367ab17bc18 91 }
Gerth 2:04ad19deeb2c 92
Gerth 2:04ad19deeb2c 93
Gerth 1:f367ab17bc18 94 if (scopedata_go==true) {
Gerth 1:f367ab17bc18 95 scopedata();
Gerth 1:f367ab17bc18 96 scopedata_go=false;
Gerth 1:f367ab17bc18 97 }
Gerth 0:3df25d5b2946 98 }
Gerth 1:f367ab17bc18 99 }