BioRobotics / Mbed 2 deprecated buttoncontrol1motor

control the x position of the elbows with buttons

Dependencies:   HIDScope QEI mbed

main.cpp@0:6bebd74a5238, 2015-09-30 (annotated)

Committer:
Gerth
Date:
Wed Sep 30 15:14:28 2015 +0000
Revision:
0:6bebd74a5238
working, now directly changes the desired angle with the buttons.; next step is to adjust te desired position and calculate the angle;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:6bebd74a5238 1 #include "mbed.h"
Gerth 0:6bebd74a5238 2 #include "QEI.h"
Gerth 0:6bebd74a5238 3 #include "HIDScope.h"
Gerth 0:6bebd74a5238 4
Gerth 0:6bebd74a5238 5 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
Gerth 0:6bebd74a5238 6 //info uit
Gerth 0:6bebd74a5238 7 HIDScope scope(3);
Gerth 0:6bebd74a5238 8
Gerth 0:6bebd74a5238 9 //encoders
Gerth 0:6bebd74a5238 10 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 0:6bebd74a5238 11
Gerth 0:6bebd74a5238 12 //ingangen
Gerth 0:6bebd74a5238 13 DigitalIn butR(D2);
Gerth 0:6bebd74a5238 14 DigitalIn butL(D3);
Gerth 0:6bebd74a5238 15
Gerth 0:6bebd74a5238 16 //uitgangen
Gerth 0:6bebd74a5238 17 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
Gerth 0:6bebd74a5238 18 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 0:6bebd74a5238 19 //PwmOut motor2_speed_control(D5);
Gerth 0:6bebd74a5238 20 //DigitalOut motor2_direction(D4);
Gerth 0:6bebd74a5238 21 const int CW=1; //clockwise
Gerth 0:6bebd74a5238 22 const int CCW=0; //counterclockwise
Gerth 0:6bebd74a5238 23
Gerth 0:6bebd74a5238 24 //frequencies
Gerth 0:6bebd74a5238 25 const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
Gerth 0:6bebd74a5238 26 const double pi_control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
Gerth 0:6bebd74a5238 27 const double read_but_frequency=25;
Gerth 0:6bebd74a5238 28
Gerth 0:6bebd74a5238 29 //tickers
Gerth 0:6bebd74a5238 30 Ticker hidscope_ticker;
Gerth 0:6bebd74a5238 31 Ticker pi_control_ticker;
Gerth 0:6bebd74a5238 32 Ticker read_but_ticker;
Gerth 0:6bebd74a5238 33
Gerth 0:6bebd74a5238 34 //constants
Gerth 0:6bebd74a5238 35 const int cpr=32*131;
Gerth 0:6bebd74a5238 36 const float PI=3.1415;
Gerth 0:6bebd74a5238 37 const float counttorad=((2*PI)/cpr);
Gerth 0:6bebd74a5238 38 const float counttodeg=(360/cpr);
Gerth 0:6bebd74a5238 39 const float mmpersecbut=(10/(read_but_frequency));
Gerth 0:6bebd74a5238 40 const float radpersecbut=(PI/read_but_frequency);
Gerth 0:6bebd74a5238 41 const float armlength=400;//length "upper arm" in mm
Gerth 0:6bebd74a5238 42
Gerth 0:6bebd74a5238 43 ///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
Gerth 0:6bebd74a5238 44
Gerth 0:6bebd74a5238 45 //DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
Gerth 0:6bebd74a5238 46 float pi_kp=0.5;
Gerth 0:6bebd74a5238 47 float pi_ki=0.01;
Gerth 0:6bebd74a5238 48 float Ts=1.0/pi_control_frequency;
Gerth 0:6bebd74a5238 49 double motor1_error_int=0;
Gerth 0:6bebd74a5238 50 double desiredangle=0;
Gerth 0:6bebd74a5238 51 double desiredposition=0;
Gerth 0:6bebd74a5238 52 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
Gerth 0:6bebd74a5238 53 //go flags
Gerth 0:6bebd74a5238 54 volatile bool scopedata_go=false, pi_control_go=false,read_but_go=false;
Gerth 0:6bebd74a5238 55
Gerth 0:6bebd74a5238 56 //acvitator functions
Gerth 0:6bebd74a5238 57
Gerth 0:6bebd74a5238 58 void scopedata_activate()
Gerth 0:6bebd74a5238 59 {
Gerth 0:6bebd74a5238 60 scopedata_go=true;
Gerth 0:6bebd74a5238 61 }
Gerth 0:6bebd74a5238 62 void pi_control_activate()
Gerth 0:6bebd74a5238 63 {
Gerth 0:6bebd74a5238 64 pi_control_go=true;
Gerth 0:6bebd74a5238 65 }
Gerth 0:6bebd74a5238 66 void read_but_activate(){read_but_go=true;}
Gerth 0:6bebd74a5238 67 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:6bebd74a5238 68
Gerth 0:6bebd74a5238 69 //scopedata
Gerth 0:6bebd74a5238 70 void scopedata()
Gerth 0:6bebd74a5238 71 {
Gerth 0:6bebd74a5238 72 scope.set(0,desiredangle);//gewenste hoek van arm
Gerth 0:6bebd74a5238 73 scope.set(1,counttodeg*encoder1.getPulses());//hoek in rad van outputshaft
Gerth 0:6bebd74a5238 74 scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
Gerth 0:6bebd74a5238 75 scope.send();
Gerth 0:6bebd74a5238 76 }
Gerth 0:6bebd74a5238 77 //////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
Gerth 0:6bebd74a5238 78
Gerth 0:6bebd74a5238 79 // Reusable PI controller
Gerth 0:6bebd74a5238 80 double pi_control( double e, double& motor1_error_int)
Gerth 0:6bebd74a5238 81 {
Gerth 0:6bebd74a5238 82 motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
Gerth 0:6bebd74a5238 83 return pi_kp*e+pi_ki*motor1_error_int;
Gerth 0:6bebd74a5238 84 }
Gerth 0:6bebd74a5238 85
Gerth 0:6bebd74a5238 86 //////////////////////////////////////////////////MAIN///////////////////////////////////
Gerth 0:6bebd74a5238 87 int main()
Gerth 0:6bebd74a5238 88 {
Gerth 0:6bebd74a5238 89 //set initial shizzle
Gerth 0:6bebd74a5238 90 motor1_speed_control.write(0);
Gerth 0:6bebd74a5238 91
Gerth 0:6bebd74a5238 92 //tickers
Gerth 0:6bebd74a5238 93 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 0:6bebd74a5238 94 pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
Gerth 0:6bebd74a5238 95 read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
Gerth 0:6bebd74a5238 96
Gerth 0:6bebd74a5238 97 while(1) {
Gerth 0:6bebd74a5238 98 //control motor 1 with a pi controller
Gerth 0:6bebd74a5238 99
Gerth 0:6bebd74a5238 100 if (read_but_go==true) {
Gerth 0:6bebd74a5238 101 float buttonR=butR.read(), buttonL=butL.read();
Gerth 0:6bebd74a5238 102 if (buttonR==0 && buttonL ==1) {
Gerth 0:6bebd74a5238 103 desiredposition += (radpersecbut);
Gerth 0:6bebd74a5238 104 read_but_go=false;
Gerth 0:6bebd74a5238 105 }
Gerth 0:6bebd74a5238 106 if (buttonR==1 && buttonL==0) {
Gerth 0:6bebd74a5238 107 desiredposition -= (radpersecbut);
Gerth 0:6bebd74a5238 108 read_but_go=false;
Gerth 0:6bebd74a5238 109 } else {
Gerth 0:6bebd74a5238 110 desiredposition=desiredposition;
Gerth 0:6bebd74a5238 111 }
Gerth 0:6bebd74a5238 112 desiredangle=desiredposition;
Gerth 0:6bebd74a5238 113 read_but_go=false;
Gerth 0:6bebd74a5238 114 }
Gerth 0:6bebd74a5238 115
Gerth 0:6bebd74a5238 116 if (pi_control_go==true) {
Gerth 0:6bebd74a5238 117 double error=desiredangle-(counttorad*encoder1.getPulses());
Gerth 0:6bebd74a5238 118 double signal_motor1=pi_control(error,motor1_error_int);//send error to p controller
Gerth 0:6bebd74a5238 119 if (signal_motor1>=0) {//determine CW or CCW rotation
Gerth 0:6bebd74a5238 120 motor1_direction.write(CW);
Gerth 0:6bebd74a5238 121 } else {
Gerth 0:6bebd74a5238 122 motor1_direction.write(CCW);
Gerth 0:6bebd74a5238 123 }
Gerth 0:6bebd74a5238 124
Gerth 0:6bebd74a5238 125 if (fabs(signal_motor1)>=1) { //check if signal is <1
Gerth 0:6bebd74a5238 126 signal_motor1=1;//if signal >1 make it 1 to not damage motor
Gerth 0:6bebd74a5238 127 } else {
Gerth 0:6bebd74a5238 128 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
Gerth 0:6bebd74a5238 129 }
Gerth 0:6bebd74a5238 130
Gerth 0:6bebd74a5238 131 motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
Gerth 0:6bebd74a5238 132 pi_control_go=false;
Gerth 0:6bebd74a5238 133 }
Gerth 0:6bebd74a5238 134 //call scopedata
Gerth 0:6bebd74a5238 135 if (scopedata_go==true) {
Gerth 0:6bebd74a5238 136 scopedata();
Gerth 0:6bebd74a5238 137 scopedata_go=false;
Gerth 0:6bebd74a5238 138 }
Gerth 0:6bebd74a5238 139 }
Gerth 0:6bebd74a5238 140 return 0;
Gerth 0:6bebd74a5238 141 }