com_servo.cpp

00001 /*
00002 // RUTINAS DE COMUNICACION CON EL SERVO
00003 // Pedro Campos 23-04-2020
00004 // 
00005 */
00006 #include "declaraciones.h"
00007 
00008 extern Serial pc;
00009 extern RawSerial serSERVO;
00010 
00011 extern DigitalOut SERVO_INC_POS;     // Salida de control INC_POS del servo (J1-2)
00012 extern DigitalOut SERVO_DEC_POS;     // Salida de control DEC_POS del servo (J1-3)
00013 
00014 extern Timer timer;
00015 
00016 extern volatile byte estado;
00017 extern enEstadoServo estado_servo;
00018 extern int16_t velocidad_servo;
00019 extern int16_t torque_servo;
00020 extern long desviacion_servo;
00021 extern struct st_datos_servo datos_servo;       // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00022 extern volatile int pos_actual_servo;
00023 extern volatile int pos_objetivo_servo;
00024 extern volatile unsigned long tempo_espera;
00025 extern int out_torque;                          // nivel de torque de TORQUE_MINIMO a TORQUE_MAXIMO
00026 extern int torque_max;                          // torque máximo calculado
00027 
00028 
00029 
00030 
00031 /*******************************************************
00032  * RUTINAS COMUNICACION SERVO PANASONIC MINAS-A5
00033  *******************************************************/
00034 
00035 
00036 void posicion_servo(void);                      // AJUSTA POSICION SERVO SI ES NECESARIO
00037 unsigned char calc_chk(char * buf, int nb);     // CALCULA CHK PARA COMUNICACION CON SERVO
00038 bool read_cmd_servo(unsigned char cmd, unsigned char *buf, byte nc);    // ENVIA COMANDO DE LECTURA A SERVO 1 y devuelve nc bytes en buf
00039 bool comunica_servo(unsigned char *buf_in, byte nb_in, unsigned char *buf_out, byte nb_out);    // ENVIA DATOS A SERVO y devuelve la respuesta
00040 bool lee_parametro_servo(byte tipo, byte numero, long *valor);          // LEE PARAMETRO DEL SERVO
00041 bool escribe_parametro_servo(byte tipo, byte numero, long valor);       // ESCRIBE PARAMETRO EN EL SERVO
00042 void flush_servo(void);                         //  * Espera que el canal serie este libre de RX y TX
00043 
00044 
00045 
00046 /**************************************************
00047  * lee estado del servo, procesa torque y actualiza
00048  **************************************************/
00049 void procesa_servo(void){
00050 byte buf_entrada[20];
00051 
00052   // LEE ESTADO SERVO
00053   if(read_cmd_servo(0x92, buf_entrada, 13)){   // SI RESPONDE
00054     estado_servo = ESTADO_SERVO_ON;
00055     velocidad_servo = buf_entrada[4]*256+buf_entrada[3];
00056     torque_servo = buf_entrada[6]*256+buf_entrada[5];
00057     unsigned long dato;
00058     dato = (unsigned long)(buf_entrada[10])*256*256*256;
00059     dato += (unsigned long)(buf_entrada[9])*256*256;
00060     dato += (unsigned long)(buf_entrada[8])*256;
00061     dato += (unsigned long)(buf_entrada[7]);
00062     desviacion_servo = (long)dato;
00063  
00064 //    pc.printf("estado:%d velocidad:%d torque:%d desviacion:%d\r\n", estado_servo, velocidad_servo, torque_servo, desviacion_servo); 
00065 //  // PROCESA ESTADO SEGUN DESVIACION_SERVO
00066 //    if(abs(desviacion_servo) <= 50){
00067 //      if(datos_servo.estado==MASTER){
00068 //        datos_servo.estado = REPOSO;
00069 //      }
00070 //    }
00071 //    else if(datos_servo.estado==REPOSO){
00072 //      datos_servo.estado=MASTER;
00073 //    }
00074 
00075     // CALCULA TORQUE DE SALIDA  segun velocidad GPS o PILOTO AUTOMATICO
00076 
00077     if(datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_RUMBO){
00078       out_torque = TORQUE_PILOTO;
00079     }
00080     else{
00081       torque_max = constrain((long)(datos_servo.velocidad*datos_servo.velocidad), 0, 12000);
00082       torque_max = map(torque_max, 0, 12000, TORQUE_MINIMO, TORQUE_MAXIMO);
00083       out_torque = map(desviacion_servo,0,DESVIACION_MAXIMA,TORQUE_MINIMO,torque_max);
00084     }
00085     escribe_parametro_servo(0, 13, out_torque);
00086 //    printf("out_torque: %d, velocidad: %.2f\r\n", out_torque, datos_servo.velocidad);
00087 
00088   }
00089   else
00090     estado_servo = ESTADO_SERVO_OFF;
00091 
00092 }
00093 
00094 
00095 
00096 /**************************************************
00097  * AJUSTA POSICION SERVO SI ES NECESARIO
00098  **************************************************/
00099 void posicion_servo(void)
00100 {
00101     static int index = 0;
00102     static bool on = false;
00103     int n_pulsos = 0;
00104 
00105 //    if((datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO) && (millis()%10)==0)    // Si ES PILOTO AUTOMATICO solo entra cada 10ms
00106 //        return;
00107 
00108 //    if(datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO)
00109 //        n_pulsos = 1;
00110 //    else
00111 //        n_pulsos = 1+((abs(pos_objetivo_servo - pos_actual_servo))/100);
00112 
00113 //    if(pos_actual_servo != pos_objetivo_servo)
00114         n_pulsos = 1;
00115 
00116     while(n_pulsos>0) {
00117         if(estado_servo == ESTADO_SERVO_ON) {  // Si el servo responde
00118             if(tempo_espera==0) {
00119                 SERVO_INC_POS = 1;
00120                 SERVO_DEC_POS = 1;
00121                 tempo_espera++;
00122             } else {
00123                 if(pos_actual_servo > pos_objetivo_servo) {
00124                     SERVO_DEC_POS = 0;
00125                     wait_us(10);
00126                     pos_actual_servo--;
00127                     tempo_espera = 0;
00128                 } else if(pos_actual_servo < pos_objetivo_servo) {
00129                     SERVO_INC_POS = 0;
00130                     wait_us(10);
00131                     pos_actual_servo++;
00132                     tempo_espera = 0;
00133                 }
00134             }
00135         }
00136         SERVO_INC_POS = 1;
00137         SERVO_DEC_POS = 1;
00138         wait_us(10);
00139         n_pulsos--;
00140     }
00141 
00142 }
00143 
00144 
00145 
00146 /**************************************************
00147  * CALCULA CHK PARA COMUNICACION CON SERVO
00148  **************************************************/
00149 unsigned char calc_chk(unsigned char * buf, int nb){
00150   unsigned char chk = 0;
00151 
00152   for(int i=0; i<nb; i++){
00153     chk += buf[i];
00154   }
00155   chk ^= 0xff;
00156   chk += 1;
00157   return chk;
00158 }
00159 
00160 
00161 
00162 /**************************************************
00163  * ENVIA COMANDO DE LECTURA A SERVO 1 y devuelve nc bytes en buf
00164  **************************************************/
00165 bool read_cmd_servo(unsigned char cmd, unsigned char *buf, byte nc){
00166 unsigned char bcmd[4];
00167 
00168     bcmd[0]=0x00;               // N. de datos
00169     bcmd[1]=0x01;               // SERVO 1
00170     bcmd[2]=cmd;                // COMANDO
00171     bcmd[3]=calc_chk(bcmd, 3);  // CHK
00172     return(comunica_servo(bcmd, 4, buf, nc));
00173 }
00174 
00175 
00176 
00177 /**************************************************
00178  * Espera que el canal serie este libre de RX y TX
00179  **************************************************/
00180 void flush_servo(void){
00181 
00182     while(serSERVO.readable()){
00183         char c = serSERVO.getc();
00184     }
00185 }
00186 
00187     
00188 
00189 /**************************************************
00190  * Espera respuesta del servo durante un tiempo en ms
00191  * devuelve false si sale por time-out
00192  **************************************************/
00193 bool espera_resp_servo(int ms){
00194 
00195     timer.reset();
00196     while(!serSERVO.readable()){
00197         if(timer.read_ms()>=ms)
00198           return false;             // sale por timeout de ms
00199     }
00200     return true;
00201 }
00202 
00203 
00204 
00205 /**************************************************
00206  * Envia n bytes del buffer por canal serie
00207  **************************************************/
00208 void envia_buffer_servo(byte *buf, int n){
00209 
00210     while(n-->0){
00211         serSERVO.putc(*buf++);
00212     }
00213 }
00214 
00215 
00216 
00217 /**************************************************
00218  * Recibe n bytes por canal serie al buffer, devuelve false si timeout
00219  **************************************************/
00220 bool recibe_buffer_servo(byte *buf, int n, int ms = 100){
00221 
00222     while(n>0){
00223         if(espera_resp_servo(ms)==false)
00224             return false;       // sale por timeout
00225         *buf++ = serSERVO.getc();    
00226         n--;
00227     }
00228     return true;
00229     
00230 }
00231 
00232 
00233 
00234 /**************************************************
00235  * ENVIA DATOS A SERVO y devuelve la respuesta
00236  **************************************************/
00237 bool comunica_servo(unsigned char *buf_in, byte nb_in, unsigned char *buf_out, byte nb_out){
00238 char c;
00239 
00240     flush_servo();
00241     serSERVO.putc(ENQ);
00242 //    pc.printf("\r\n");
00243 //    pc.printf("envia ENQ\r\n");
00244     if(espera_resp_servo(100)==false){    // espera respuesta durante 100ms
00245         pc.printf("sin respuesta\r\n");
00246         return false;                     // sale por timeout de 100ms
00247     }
00248     c = serSERVO.getc(); 
00249     if(c != EOT){             // Si no es correcta la respuesta
00250         pc.printf("no EOT c=%02X\r\n", c);    
00251         return false;                     // sale por respuesta incorrecta
00252     }
00253     envia_buffer_servo(buf_in, nb_in);    // Envia mensaje de comando
00254 //    pc.printf("envia buf_in\r\n");
00255     if(espera_resp_servo(100)==false){    // espera respuesta durante 100ms
00256         pc.printf("sin respuesta\r\n");
00257         return false;                     // sale por timeout de 100ms
00258     }
00259     c=serSERVO.getc();
00260     if(c != ACK){             // Si no es correcta la respuesta
00261         pc.printf("no ACK c=%02X\r\n", c);    
00262         return false;                     // sale por respuesta incorrecta
00263     }
00264     if(espera_resp_servo(100)==false){    // espera respuesta durante 100ms
00265         pc.printf("sin respuesta\r\n");
00266         return false;                     // sale por timeout de 100ms
00267     }
00268     c=serSERVO.getc();
00269     if(c!=ENQ){             // Si no es correcta la respuesta
00270         pc.printf("no ENQ c=%02X\r\n", c);    
00271         return false;                     // sale por respuesta incorrecta
00272     }
00273     serSERVO.putc(EOT);
00274 //    pc.printf("envia EOT\r\n");
00275     if(recibe_buffer_servo(buf_out, nb_out)==false){       // Espera datos nb_out
00276         pc.printf("sin respuesta\r\n");
00277         return false;                       //timeout en respuesta
00278     }
00279     byte chk = calc_chk(buf_out, nb_out-1); // CHK
00280     if(chk == buf_out[nb_out-1]){           // Datos recibidos
00281         serSERVO.putc(ACK);                 // Envia ACK
00282 //        pc.printf("CHK OK, envia ACK\r\n");
00283         return true;                        // Finaliza OK
00284     }
00285     serSERVO.putc(NAK);                 // Envia NAK, error en datos 
00286     pc.printf("CHK NO OK, envia NAK\r\n");
00287     return false;                       // Finaliza con error
00288 }
00289 
00290 
00291 
00292 /**************************************************
00293  * LEE PARAMETRO DEL SERVO
00294  **************************************************/
00295 bool lee_parametro_servo(byte tipo, byte numero, long *valor){
00296 unsigned char bcmd[12];
00297 unsigned char bresp[12];
00298 unsigned long dato;
00299 
00300     bcmd[0]=0x02;               // N. de datos
00301     bcmd[1]=0x01;               // SERVO 1
00302     bcmd[2]=0x07;               // COMANDO lectura de parametro
00303     bcmd[3]=tipo;               // tipo de parametro
00304     bcmd[4]=numero;             // número de parametro
00305     bcmd[5]=calc_chk(bcmd, 5);  // CHK
00306     if(comunica_servo(bcmd, 6, bresp, 9)){
00307       dato = (unsigned long)(bresp[6])*256*256*256;
00308       dato += (unsigned long)(bresp[5])*256*256;
00309       dato += (unsigned long)(bresp[4])*256;
00310       dato += (unsigned long)(bresp[3]);
00311       *valor = (long)dato;
00312       
00313       return true;
00314     }
00315     else
00316       return false;
00317 }
00318 
00319 
00320 
00321 /**************************************************
00322  * ENVIA PARAMETRO AL SERVO
00323  **************************************************/
00324 bool escribe_parametro_servo(byte tipo, byte numero, long valor){
00325 unsigned char bcmd[12];
00326 unsigned char bresp[12];
00327 unsigned long dato;
00328 
00329     bcmd[0]=0x06;               // N. de datos
00330     bcmd[1]=0x01;               // SERVO 1
00331     bcmd[2]=0x17;               // COMANDO escritura de parametro
00332     bcmd[3]=tipo;               // tipo de parametro
00333     bcmd[4]=numero;             // número de parametro
00334 
00335     dato = (unsigned long)valor;
00336 
00337     bcmd[5]=(unsigned char)(valor&0x000000ff);             // valor low de parametro
00338     bcmd[6]=(unsigned char)((valor>>8)&0x000000ff);        // valor de parametro
00339     bcmd[7]=(unsigned char)((valor>>16)&0x000000ff);       // valor de parametro
00340     bcmd[8]=(unsigned char)((valor>>24)&0x000000ff);       // valor high de parametro
00341 
00342     bcmd[9]=calc_chk(bcmd, 9);  // CHK
00343     if(comunica_servo(bcmd, 10, bresp, 5)){      
00344       return true;
00345     }
00346     else
00347       return false;
00348 }
00349 
00350 
00351 
00352