main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "EthernetInterface.h"
00004 #include "ExperimentServer.h"
00005 #include "QEI.h"
00006 #include "MotorShield.h" 
00007 #include "HardwareSetup.h"
00008 //#include<iostream> // to use min function
00009 //#include<algorithm>// to use min function 
00010 //using namespace std; 
00011 
00012 #define NUM_INPUTS  5
00013 #define NUM_OUTPUTS 5
00014 #define DT 0.001
00015 
00016 
00017 #ifndef M_PI
00018 #define M_PI           3.14159265358979323846
00019 #endif
00020 
00021 #define Vnom           12.0
00022 
00023 
00024 Serial pc(USBTX, USBRX);    // USB Serial Terminal
00025 ExperimentServer server;    // Object that lets us communicate with MATLAB
00026 Timer t;                    // Timer to measure elapsed time of experiment
00027 
00028 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00029 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00030 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING);  // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00031 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
00032 
00033 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ
00034 
00035 int main (void)
00036 {
00037     // Link the terminal with our server and start it up
00038     server.attachTerminal(pc);
00039     server.init();
00040     float V; //input voltage
00041     float d; // duty cycle
00042     float error;
00043     float last_error=0;
00044     float sum_error=0;
00045     float duration =3;
00046     int cst;
00047     // Continually get input from MATLAB and run experiments
00048     float input_params[NUM_INPUTS];
00049     pc.printf("%f",input_params[0]);
00050     
00051     while(1) {
00052         if (server.getParams(input_params,NUM_INPUTS)) {
00053             float theta_d   = input_params[0]; // desired angle degrees
00054             float Kp   = input_params[1]; // Kp
00055             float Kd   = input_params[2]; // Kp
00056             float Ki   = input_params[3]; // Kp
00057             float duration = input_params[4]; //
00058             int dir;
00059 
00060             // Setup experiment
00061             t.reset();
00062             t.start();
00063             encoderA.reset();
00064             encoderB.reset();
00065             encoderC.reset();
00066             encoderD.reset();
00067 
00068             motorShield.motorAWrite(0, 0); //turn motor A off
00069             
00070             //use the motor shield as follows:
00071             //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
00072              
00073             // Run experiment
00074             
00075             
00076             /*while( t.read() < 2 ) {
00077                 // Perform control loop logic
00078                 if (t.read() < 1)
00079                     motorShield.motorAWrite(0.5, 0); //run motor A at "v1" duty cycle and in the forward direction 
00080                 else
00081                     motorShield.motorAWrite(0.5, 1); //run motor A at "v2" duty cycle and in the forward direction */
00082 
00083                 // Form output to send to MATLAB
00084                 float output_data[NUM_OUTPUTS];
00085     /*            output_data[0] = t.read();
00086                 output_data[1] = encoderA.getPulses()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
00087                 output_data[2] = encoderA.getVelocity()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
00088                 output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
00089                 output_data[4] = V_nom*0.5;*/
00090                 
00091                 
00092                 
00093                 
00094                   while( t.read() < duration ) {
00095                     output_data[0] = t.read();
00096                     output_data[1] = encoderA.getPulses()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
00097                     output_data[2] = encoderA.getVelocity()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
00098                     output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
00099                     
00100                     
00101                     error = (theta_d-output_data[1]);
00102                     
00103                     
00104                     V = Kp*error + Kd*(error-last_error)/DT + Ki*sum_error; //(-output_data[2]); //PID controller gives voltage to apply
00105                     //output_data[4] = V;
00106                     last_error = error;
00107                     sum_error += error;
00108                     // convert to duty cycle
00109                     if (V>0){
00110                         dir = 0;
00111                         d = V/Vnom;
00112                         cst=1;
00113                         }
00114                     else { // V<=0;
00115                         dir = 1;
00116                         d = -V/Vnom;// make sure this is positive
00117                         cst = -1;
00118                         //
00119                         }
00120                     
00121                     if (d>=1){
00122                         d=1;
00123                         }
00124                         
00125                   
00126                     output_data[4]=d*Vnom*cst;
00127                     //d = std::min(d,1) // ceil the value of the duty cycle;
00128                     
00129                 // Perform control loop logic
00130                      //if (t.read() < 1)
00131                         motorShield.motorAWrite(d, dir); //run motor A at "v1" duty cycle and in the forward direction 
00132                     // else
00133                         
00134 
00135                 
00136                 
00137                 
00138                 // Send data to MATLAB
00139                 server.sendData(output_data,NUM_OUTPUTS);
00140                 
00141                 wait(DT); //run control loop at 1kHz
00142             }
00143             // Cleanup after experiment
00144             server.setExperimentComplete();
00145             motorShield.motorAWrite(0, 0); //turn motor A off
00146         } // end if
00147     } // end while
00148     
00149 } // end main
00150