main.cpp

00001 #include "mbed.h"
00002 #include "L6206.h"
00003 #include "BDCMotor.h"
00004 #include <math.h>
00005 #include "QEI.h"
00006 
00007 #define v_max 15 //[mm/s]
00008 #define a_max 7.5f
00009 #define pulsesPerRev 45
00010 #define a 135.1 //Distanza foro attuatore lineare- parte superiorie del braccio [mm]
00011 #define b 482.25 //distanza foro superiore braccio- foro inferiore attuatore [mm]
00012     
00013     float v_normalized = 0; //Manual command that indicates the direction and the magnitude of the velocity
00014     float v_actual = 0; //real velocity of the arm, computed through as incremental ratio
00015     float position=0; 
00016     float prev_position=0;
00017     float linear_position=0;
00018     float prev_linear_position=0;
00019     float delta_v;
00020     
00021     float k = 0;
00022     float pos_encoder = 0; //Posizione misurata dall'encoder
00023     float k1 = 0;
00024     float resolution; //sampling  = resolution of the encoder
00025     int Auto = 0;
00026     int t=0;
00027 
00028 Thread thread;
00029 QEI encoder(PB_4, PB_5,NC, pulsesPerRev, QEI::X4_ENCODING); //Check the datasheet to know what is the pulses for revolution value
00030 //From the library it requires to pass NC in the index space in the case it is not present
00031 
00032 InterruptIn end0(PC_1);
00033 
00034 DigitalOut led1(LED1);
00035 InterruptIn button(USER_BUTTON);
00036 
00037 
00038 
00039 L6206 *motor;
00040 
00041 //L6206 L6206(PinName EN_A,PinName  EN_B,PinName IN_1A,PinName IN_2A,PinName IN_1B,PinName IN_2B) ;
00042 //sdd::Cout
00043 
00044 void end0_int_handler(float position, float k, float k1) //10 mm back
00045 {
00046     while ((10-linear_position)>0) {
00047     motor->run(1, BDCMotor::BWD);                        
00048 
00049     k1 = k;
00050     k = ((10-linear_position)/5)*100;
00051     //PWM_duty = PWM_dutyReduct*(1-k)
00052     motor->set_speed(k1,k);                
00053         
00054     pos_encoder=encoder.getPulses();
00055     
00056     position = prev_position + pos_encoder;       
00057             
00058     linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
00059 
00060     }
00061 
00062 }
00063 
00064 void go_to_zero( float EncoderResolution, int pos_encoder, int k, int k1, int t) {
00065     motor->run(1, BDCMotor::BWD);
00066         k=0;
00067     while (t != 3) {
00068         k1 = k;
00069         k = k+a_max/EncoderResolution*100/v_max;
00070         motor->set_speed(k1,k);
00071         pos_encoder = encoder.getPulses();
00072 
00073         if (pos_encoder==0) {
00074             t++;
00075         } else {
00076             t=0;
00077         }
00078     }
00079 }
00080 
00081 //Pin can: CANRX PB8 and CANTX PB9
00082 
00083 const PinName can1rxPins[] = {PB_8};
00084 const PinName can1txPins[] = {PB_9};
00085 
00086 CAN can1(can1rxPins[0], can1txPins[0]);
00087 
00088 CANMessage messageIn;
00089 CANMessage messageOut;
00090 
00091 void canRxIsr() {
00092     while(1) {
00093 
00094     if(can1.read(messageIn))
00095      
00096         {
00097             led1 = !led1;
00098             printf("received\n\r");
00099         }
00100     }
00101 }
00102 void command() {
00103     
00104     canRxIsr();
00105     wait(0.5);
00106 }
00107 
00108 void sendMessage() {
00109     int status = can1.write(messageOut);
00110     printf("Send status: %d\r\n", status);
00111 }
00112 
00113 void controller(float v_normalized, float v_actual, float resolution, int pos_encoder, float position) {
00114     
00115     
00116     float v_request = abs(v_normalized*v_max); //*(100/v_max); //Command is think to be a normalized velocity
00117     delta_v = v_request-v_actual;
00118     float k=0;
00119 
00120     while (delta_v !=  0) {
00121         /*Choosing a maximum acceleration, preferebly linked to the condition of the syste (motor, structure, Pwm and so on).
00122         Imposing an acceleration of 7.5 mm/s^2 to reach the v_max 2  secs are needed.*/
00123         //Measure the new position
00124         
00125         if (v_normalized > 0){
00126             motor->run(1, BDCMotor::FWD);
00127             float k1 = k;
00128             k = k+ a_max/( resolution)*100/(v_max);
00129             motor->set_speed(k1,k);
00130                    
00131         } else if (v_normalized<0){
00132             motor->run(1, BDCMotor::BWD);
00133             float k1 = k;
00134             k = k+a_max/resolution*100/v_max;
00135             motor->set_speed(k1,k);
00136                      
00137         } else{
00138             float k1 = k;
00139             k = k-a_max/resolution*100/v_max;
00140             motor->set_speed(k1,k);
00141                 
00142         }
00143             pos_encoder = encoder.getPulses();
00144 
00145 
00146 
00147             position = prev_position + pos_encoder;
00148             prev_position = position;
00149             
00150             linear_position= sqrt(a*a+b*b-2*a*b*cos(position));
00151 
00152             v_actual = (prev_linear_position-linear_position)*resolution;
00153 
00154             delta_v = v_request-v_actual;
00155             
00156             prev_linear_position=linear_position;
00157 
00158             wait(0.1);
00159     }
00160              
00161 }
00162 
00163 void controller_ausiliar (){
00164     controller(v_normalized, v_actual, resolution, pos_encoder, position);
00165 }
00166 
00167 int main (int argc,char **argv) {
00168        
00169     //Prendo il comando -> fisso un punto da raggiungere (Automatico) -> In tot step voglio tale duty cicle
00170     //Common variables
00171 
00172     //Auto variables
00173     float finalDestination; //posizione da raggiungere RICEVUTA COME PARAMETRO
00174     float acceleration_range = 15; //n° step in accelerazione/decelerazione. Deciso da me 
00175     float track_error = 1;
00176     float PWM_dutyReduct = 0; //Variables used in case of triangular control
00177     float acc_rangeReduct = 0;
00178     float Contr_reference = 0; //Is the starting tracking error, needed to impose the right form  of controller
00179     float sens_error;
00180     //Manual variables
00181  
00182     go_to_zero(resolution ,pos_encoder , k, k1, t);
00183 
00184     //end0.rise(&end0_int_handler);
00185     
00186 
00187     //Absolute position of the actuator (from can?)
00188     
00189             switch (Auto) {
00190 
00191             case 1: {/*The rover is in its automatic mode*/
00192 
00193                 //OTTENGO LA posizione iniziale dell'encoder startPos_encoder
00194                 //Ottengo il comando 
00195                 Contr_reference = finalDestination-position;
00196                 track_error = Contr_reference;
00197                 while (track_error>sens_error) {
00198                     
00199                     if (Contr_reference>2*acceleration_range) { //Controllo trapezoidale 
00200                         
00201                         if (track_error>(finalDestination-acceleration_range)) {
00202                             k1 = k;
00203                             k = (track_error/(finalDestination-acceleration_range))*100; //V da imporre
00204                             motor->set_speed(k1,k);
00205                            
00206 
00207                                 } else if (track_error<acceleration_range) {
00208                                 
00209                             k1 = k;
00210                             k = ((track_error)/acceleration_range)*100; //PWM_duty = 90*(1-k)
00211                             motor->set_speed(k1,k);
00212                            
00213                         } 
00214 
00215                         //Leggo la pos_encoder attuale
00216                             
00217                     } else { //Controllo triangolare
00218                         /*Si agisce in maniera analoga al caso non saturato ma si impone un duty cicle massimo minore
00219                         In particolare la v_max è proporzionale alla dimensione dello spostamento con 90 che corrisponde
00220                         a un intervallo pari a 40 e poi si defininisce il nuovo PWM_dutyReduct = interval*90/40 
00221                         */
00222 
00223                         PWM_dutyReduct = (Contr_reference/(2*acceleration_range))*100;
00224                              
00225                         //A new variable is needed to indicate the range of acceleration (in this case is less than the acceleration_range)
00226                         acc_rangeReduct = Contr_reference/2;
00227                         if (track_error>acc_rangeReduct) {
00228                             k1 = k;
00229                             k = (track_error/(finalDestination-acc_rangeReduct))*PWM_dutyReduct; //costante di proporzionalità
00230                             //PWM_duty = k1*PWM_dutyReduct
00231                             motor->set_speed(k1,k);
00232                         
00233                         } else {
00234                             k1 = k;
00235                             k = (track_error/acc_rangeReduct)*PWM_dutyReduct;
00236                             //PWM_duty = PWM_dutyReduct*(1-k)
00237                             motor->set_speed(k1,k);
00238                           
00239                         }
00240                         //Leggo la pos_encoder attuale
00241                     }
00242                 track_error = finalDestination-pos_encoder; //It is used as counter
00243                 }
00244             }
00245             break;
00246 
00247             default: { //Manual control based on a velocity control with a position feedback. The following velocities are gotten through a [-1,1] command,
00248                 //normalized velocity
00249                 
00250                 thread.start(&controller_ausiliar);
00251                 thread.start(command);
00252 
00253                 while(1){
00254 
00255                     
00256                     sendMessage(); //position
00257                             
00258                 }
00259                     
00260             break;
00261             
00262             }
00263         }
00264      }