main.cpp

00001 #include "mbed.h"
00002 #include "encoder.h"
00003 #include "HIDScope.h"
00004 #include "QEI.h"
00005 #include "MODSERIAL.h"
00006 
00007 DigitalOut  motor1_direction(D4);
00008 PwmOut      motor1_speed(D5);
00009 PwmOut      led(D9);
00010 DigitalIn   button_1(PTC6); //counterclockwise
00011 DigitalIn   button_2(PTA4); //clockwise
00012 Encoder     motor1(D12,D13);
00013 HIDScope    scope(1);
00014 AnalogIn PotMeter1(A1);
00015 Ticker controller;
00016 Ticker ticker_regelaar;
00017 
00018 MODSERIAL   pc(USBTX, USBRX);
00019 volatile bool regelaar_ticker_flag;
00020 
00021 void setregelaar_ticker_flag()
00022 {
00023     regelaar_ticker_flag = true;
00024 }
00025 
00026 #define SAMPLETIME_REGELAAR 0.005
00027 
00028 //define states
00029 enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
00030 uint8_t state = GOTOHOMEPOSITION;
00031 
00032 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
00033 const double g = 360; // Aantal graden 1 rotatie
00034 const double c = 4200; // Aantal counts 1 rotatie
00035 const double q = c/(g);
00036 
00037 //PI-controller constante
00038 const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
00039 const double motor1_Ki = 0.002; // Integrating gain m1.
00040 const double motor1_Ts = 0.01; // Time step m1
00041 double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
00042 
00043 // Reusable P controller
00044 double Pc (double error, const double Kp)
00045 {
00046     return motor1_Kp * error;
00047 }
00048 
00049 // Measure the error and apply output to the plant
00050 void motor1_controlP()
00051 {
00052     double referenceP1 = PotMeter1.read();
00053     double positionP1 = q*motor1.getPosition();
00054     double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
00055 }
00056 
00057 // Reusable PI controller
00058 double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
00059 {
00060     err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
00061     return motor1_Kp*error + motor1_Ki*err_int;
00062 } // De totale fout die wordt hersteld met behulp van PI control.
00063 
00064 void motor1_controlPI()
00065 {
00066     double referencePI1 = PotMeter1.read();
00067     double positionPI1 = q*motor1.getPosition();
00068     double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
00069 }
00070 
00071 const int pressed = 0;
00072 
00073 double H;
00074 double P;
00075 double D;
00076 
00077 
00078 void sethome(){
00079     H = motor1.getPosition();
00080 }
00081 
00082 void move_motor1_ccw (){
00083     motor1_direction = 0;
00084     motor1_speed = 0.8;
00085 }
00086 
00087 void move_motor1_cw (){
00088     motor1_direction = 1;
00089     motor1_speed = 0.8;
00090 }
00091 
00092 void movetohome(){
00093     P = motor1.getPosition();
00094     D = (P - H);
00095  
00096     if (D >= -36 && D <= 36){
00097         motor1_speed = 0;
00098     }
00099     else if (D > 24){
00100         motor1_direction = 1;
00101         motor1_speed = 0.1;
00102     }
00103     else if (D < -24){
00104         motor1_direction = 0;
00105         motor1_speed = 0.1;
00106     }
00107 }
00108 
00109 void move_motor1()
00110 {
00111     if (button_1 == pressed) {
00112         move_motor1_cw ();
00113     } else if (button_2 == pressed) {
00114         move_motor1_ccw ();
00115     } else { 
00116         motor1_speed = 0;
00117     }
00118 }
00119 
00120 void read_encoder1 ()    // aflezen van encoder via hidscope??
00121 {
00122     scope.set(0,motor1.getPosition());
00123     led.write(motor1.getPosition()/100.0);
00124     scope.send();
00125     wait(0.2f);
00126 }
00127 
00128 int main()
00129 {    
00130     while (true) {
00131         pc.baud(9600);          //pc baud rate van de computer
00132         
00133     switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
00134         
00135         case GOTOHOMEPOSITION:      //positie op 0 zetten voor arm 1
00136          {
00137             pc.printf("gotohomeposition\n\r");
00138             read_encoder1();
00139             sethome();
00140             state = MOVE_MOTOR;
00141             break;
00142         }
00143         
00144         case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
00145         {
00146             pc.printf("move_motor\n\r");
00147             while (state == MOVE_MOTOR){
00148             pc.printf("whiletrue\n\r");
00149             move_motor1();
00150             if (button_1 == pressed && button_2 == pressed){
00151             state = BACKTOHOMEPOSITION;
00152             }
00153             }
00154             wait (1);
00155             break; 
00156         }
00157         
00158         case BACKTOHOMEPOSITION:    //motor gaat terug naar homeposition
00159         {
00160             pc.printf("backhomeposition\n\r");
00161             wait (1);
00162             
00163             ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
00164             
00165             while(state == BACKTOHOMEPOSITION){
00166                 movetohome();
00167                 while(regelaar_ticker_flag != true);
00168                 regelaar_ticker_flag = false;
00169                 
00170                 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
00171                 pc.printf("referentie %d\n\r", H);
00172                 
00173                 if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
00174                 motor1.setPosition(0);
00175                 H = motor1.getPosition();
00176                 state = STOP;
00177                 break;
00178                 }
00179             }
00180         }
00181         case STOP:
00182         {
00183            const int einde = 1;
00184            while(state == STOP){
00185             motor1_speed = 0;
00186             if (einde == 1){
00187             pc.printf("homepositie %d\n\r", H);
00188             pc.printf("huidige positie %d\n\r", P);
00189             pc.printf("einde\n\r");
00190             
00191             }
00192                }
00193             break;
00194             }
00195 }
00196 }
00197 }