home *** CD-ROM | disk | FTP | other *** search
/ CD Actual 45 / cda45.iso / VNULabs / Microcontroladores / stump2.c < prev   
Encoding:
C/C++ Source or Header  |  2000-04-01  |  13.0 KB  |  365 lines
  1. // ********************************************************
  2. //  Programa para control del Hexßpodo                    *
  3. //  Versi≤n 1.0                                           *
  4. //  (c) Luis Asensio Morcillo y Eugenio Martφn Cuenca     *
  5. //  Enero del 2.000                                       *
  6. // ********************************************************
  7.  
  8. #define TOUCHED 0
  9. #define LEFT_EYE 3
  10. #define RIGHT_EYE 1
  11.  
  12. #define PULSES 20           //N·mero de pulsos que se le envφa a cada servo
  13. #define TIME_DELAY 20       //Retardo en ms entre pulsos
  14. #define MODE 0              //PIN puerto B que indica el mode para andar
  15.  
  16. #define LEFT_SERVO_PIN 5    //Pin de salida que ataca al servo izquierdo
  17. #define CENTER_SERVO_PIN 6  //Pin de salida que ataca al servo central
  18. #define RIGHT_SERVO_PIN 7   //Pin de salida que ataca al servo derecho
  19.  
  20. #define LEFT_FEELER 4       //Pin de entrada del sensor izquierdo
  21. #define RIGHT_FEELER 2      //Pin de entrada del sensor derecho
  22.  
  23. #define CHECK_FEELERS 0         //Comando para la rutina walk.
  24. #define DO_NOT_CHECK_FEELERS 1  //Comando para la rutina walk.
  25.  
  26. #define NO_FEELER_TOUCHED  0    //Valor que devuelve la rutina feelers()
  27. #define LEFT_FEELER_TOUCHED 1   //Valor que devuelve la rutina feelers()
  28. #define RIGHT_FEELER_TOUCHED 2  //Valor que devuelve la rutina feelers()
  29.  
  30. #define ADD_PORTB 0x06          //Direcci≤n del puerto B
  31. #define puerto 0x06
  32.  
  33. #define CENTER 150              //Valor en unidades de 100 us para colocar el servo en el centro.
  34.  
  35. char counter;    //Contador
  36. char counter2;   //Contador
  37.  
  38. char Cleg_max;
  39. char Cleg_min;
  40. char error_code; //Valor de vuelta
  41.  
  42. #define Rleg_max 185    //Posiciones de los servos en unidades de 100 us.
  43. #define Rleg_min 130
  44. #define Lleg_max 170
  45. #define Lleg_min 115
  46.  
  47. #define Retrasa_patas_izquierdas 170
  48. #define Avanza_patas_derechas 185
  49. #define Avanza_patas_izquierdas 115
  50. #define Retrasa_patas_derechas 130
  51.  
  52. #define Apoya_patas_izquierdas Cleg_min
  53. #define Apoya_patas_derechas Cleg_max
  54.  
  55. char feelers(void);
  56. void walk_back(void);
  57. char walk(char left_servo, char center_servo, char right_servo, char check_feelers);
  58. void delay_dms(char duracion);
  59. void calibrate(void);
  60.  
  61.  
  62.  
  63. void main (void)
  64. {
  65.     //Inicializaci≤n del Hardware
  66.     set_bit( STATUS, RP0 );     //Selecciona banco de registros 1
  67.     set_tris_a( 0 );            //Configura el puerto A
  68.     set_tris_b( 21 );           //Configura el puerto B
  69.     OPTION_REG = 71;            //Configura el registro OPTION
  70.     clear_bit( STATUS, RP0 );   //Selecciona banco de registros 1
  71.     INTCON = 160;               //Configura el registro INTCON
  72.  
  73.     //PIN que controlan los servos a estado bajo
  74.     clear_bit(PORTB, RIGHT_SERVO_PIN);
  75.     clear_bit(PORTB, CENTER_SERVO_PIN);
  76.     clear_bit(PORTB, LEFT_SERVO_PIN);
  77.     if ( input_pin_port_b( MODE ) )     //Selecciona el modo de funcionamiento para andar
  78.     {
  79.         Apoya_patas_derechas = 157;
  80.         Apoya_patas_izquierdas = 143;                 //Elevaci≤n mas baja
  81.     }
  82.     else
  83.     {
  84.         Apoya_patas_derechas = 165;                 //Elevaci≤n mßs alta
  85.         Apoya_patas_izquierdas = 135;
  86.     }
  87.     
  88.     
  89.     if (input_pin_port_b( LEFT_FEELER ) == 0) calibrate();//Sensor izquierdo activo ?(es activo en baja)
  90.     
  91.     error_code = NO_FEELER_TOUCHED; //Indica que no hay ning·n sensor activo.
  92.     while (1)    //bucle principal
  93.     {
  94.         
  95.         switch (error_code)
  96.         {
  97.             case NO_FEELER_TOUCHED:     //Sigue sin haber sensores activos
  98.             {
  99.                 set_bit(PORTB, LEFT_EYE);       //Enciende el led izquierdo
  100.                 clear_bit(PORTB, RIGHT_EYE);    //Apaga el el led derecho
  101.                 error_code = walk(Retrasa_patas_izquierdas,Apoya_patas_izquierdas,Avanza_patas_derechas,CHECK_FEELERS);
  102.                 if (error_code == NO_FEELER_TOUCHED)    //Sigue sin haber sensores activos.
  103.                 {
  104.                     clear_bit(PORTB, LEFT_EYE); //Apaga el ojo izquierdo
  105.                     set_bit(PORTB, RIGHT_EYE);  //Enciende el derecho
  106.                     error_code = walk(Avanza_patas_izquierdas,Apoya_patas_derechas,Retrasa_patas_derechas,CHECK_FEELERS);
  107.                 }
  108.                 break;
  109.             }
  110.             
  111.             case LEFT_FEELER_TOUCHED:   //Se ha activado el sensor izquierdo
  112.             {
  113.                 walk_back();            //Camina hacia atrßs
  114.                 set_bit(PORTB, LEFT_EYE);   //Enciende el ojo izquierdo
  115.                 clear_bit(PORTB, RIGHT_EYE);//Apaga el ojo derecho
  116.                 for (counter2=1;counter2<=4 ;counter2++)  //Gira hacia al lado correspondiente
  117.                 {
  118.                      error_code = walk(Retrasa_patas_izquierdas,Apoya_patas_izquierdas,Retrasa_patas_derechas,CHECK_FEELERS);
  119.                      if (error_code != NO_FEELER_TOUCHED) break;    //Si hay alg·n sensor activo sale
  120.                      error_code = walk(Avanza_patas_izquierdas,Apoya_patas_derechas,Avanza_patas_derechas,CHECK_FEELERS);
  121.                      if (error_code != NO_FEELER_TOUCHED) break;    //Si hay alg·n sensor activo sale
  122.                 }
  123.                 break;
  124.             }
  125.             
  126.             case RIGHT_FEELER_TOUCHED:  //Se ha activado el sensor derecho
  127.             {
  128.                 walk_back();            //Camina hacia atrßs
  129.                 clear_bit(PORTB, LEFT_EYE); //Apaga el ojo izquierdo
  130.                 set_bit(PORTB, RIGHT_EYE);  //Enciende el ojo derecho
  131.                 for (counter2=1;counter2<=4 ;counter2++)    //Gira hacia el lado correspondiente.
  132.                 {
  133.                      error_code = walk(Retrasa_patas_izquierdas,Apoya_patas_derechas,Retrasa_patas_derechas,CHECK_FEELERS);
  134.                      if (error_code != NO_FEELER_TOUCHED) break;    //Si hay alg·n sensor activo sale
  135.                      error_code = walk(Avanza_patas_izquierdas,Apoya_patas_izquierdas,Avanza_patas_derechas,CHECK_FEELERS);
  136.                      if (error_code != NO_FEELER_TOUCHED) break;    //Si hay alg·n sensor activo sale
  137.                 }
  138.                 break;
  139.             }
  140.         
  141.         }//del switch
  142.         
  143.     }//del while
  144.     
  145. }
  146.  
  147. //**************************************************************************//
  148. //Funci≤n feelers                                                           //
  149. //**************************************************************************//
  150. //Entradas:     nada:
  151. //
  152. //Devuelve:     el sensor que estß activo; si lo hay.
  153. //
  154. //Nota: Los sensores son activos en baja.
  155. //**************************************************************************//
  156. char feelers(void)
  157. {
  158.     if (input_pin_port_b(LEFT_FEELER)== 0)
  159.     {
  160.         return LEFT_FEELER_TOUCHED;
  161.                 
  162.      } 
  163.     if (input_pin_port_b(RIGHT_FEELER)== 0)
  164.     {
  165.         return RIGHT_FEELER_TOUCHED;
  166.         
  167.     }
  168.     return NO_FEELER_TOUCHED;
  169. }    
  170. //**************************************************************************//
  171.  
  172. //**************************************************************************//
  173. //Procedimiento: pulse_out                                                           //
  174. //**************************************************************************//
  175. //Entradas:     char pin:       pin por donde envφa el pulso
  176. //              char duraci≤n:  tiempo en unidades de 10 us
  177. //Devuelve:     nada
  178. //
  179. //Nota:     El pulso el positivo:(estado bajo, estado alto, estado bajo)
  180. //**************************************************************************//
  181. void pulse_out(char pin, char duracion)
  182. {
  183.     
  184.     switch (pin)
  185.     {
  186.     
  187.         case 0:{
  188.             clear_bit(puerto,0);
  189.             set_bit(puerto,0);
  190.             delay_dms(duracion);
  191.             clear_bit(puerto,0);
  192.             return;   
  193.         }
  194.         case 1:{
  195.             clear_bit(puerto,1);
  196.             set_bit(puerto,1);
  197.             delay_dms(duracion);
  198.             clear_bit(puerto,1);
  199.             return;
  200.         }
  201.         case 2:{
  202.             clear_bit(puerto,2);
  203.             set_bit(puerto,2);
  204.             delay_dms(duracion);
  205.             clear_bit(puerto,2);
  206.             return;
  207.         }
  208.         case 3:{
  209.             clear_bit(puerto,3);
  210.             set_bit(puerto,3);
  211.             delay_dms(duracion);
  212.             clear_bit(puerto,3);
  213.             return;
  214.         }
  215.         case 4:{
  216.             clear_bit(puerto,4);
  217.             set_bit(puerto,4);
  218.             delay_dms(duracion);
  219.             clear_bit(puerto,4);
  220.             return;
  221.         }
  222.         case 5:{
  223.             clear_bit(puerto,5);
  224.             set_bit(puerto,5);
  225.             delay_dms(duracion);
  226.             clear_bit(puerto,5);
  227.             return;
  228.         }
  229.         case 6:{
  230.             clear_bit(puerto,6);
  231.             set_bit(puerto,6);
  232.             delay_dms(duracion);
  233.             clear_bit(puerto,6);
  234.             return;
  235.         }
  236.         case 7:{
  237.             clear_bit(puerto,7);
  238.             set_bit(puerto,7);
  239.             delay_dms(duracion);
  240.             clear_bit(puerto,7);
  241.             return;
  242.         }
  243.     }
  244.     
  245.     
  246. }
  247.  
  248. //**************************************************************************//
  249. //Funci≤n: walk                                                        //
  250. //**************************************************************************//
  251. //Entradas:     char left_servo:        posicion del servo IZQUIERDO
  252. //              char center_servo:      posicion del servo CENTRAL
  253. //              char right_servo:       posicion del servo DERECHO
  254. //              char feelers_checking:   indica si tiene que comprobar los sensores
  255. //
  256. //Devuelve:     Si se activado alg·n sensor (lo mismo que feelers)
  257. //
  258. //Nota:     La posici≤n de los servos se da en unidades de 10 us.
  259. //**************************************************************************//
  260.  
  261. char walk(char left_servo, char center_servo, char right_servo, char feelers_checking)
  262. {
  263.      char walk1;        //Almacena temporalmente el resultado de feelers()
  264.      //Se envφan total numero PULSES de pulsos.
  265.      for (counter = 0; counter < PULSES; counter++)
  266.      {
  267.         pulse_out( CENTER_SERVO_PIN, center_servo);//Mueve el servo central
  268.         delay_ms(TIME_DELAY);       //Retardo de TIME_DELAY ms
  269.         //Si hay que comprobar los sensores se hace
  270.         if (feelers_checking == CHECK_FEELERS)
  271.         {
  272.             walk1 = feelers();
  273.             if (walk1 != NO_FEELER_TOUCHED) return walk1; //Si hay sensor activo se sale
  274.         }
  275.         
  276.      }
  277.      //Se envφan total numero PULSES de pulsos.        
  278.      for (counter = 0; counter < PULSES; counter++)
  279.      {
  280.         //Da un paso
  281.         pulse_out( RIGHT_SERVO_PIN, right_servo);
  282.         pulse_out( LEFT_SERVO_PIN, left_servo);
  283.         pulse_out( CENTER_SERVO_PIN, center_servo);
  284.         delay_ms(TIME_DELAY);
  285.         //Si hay que comprobar los sensores, se hace.
  286.         if (feelers_checking == CHECK_FEELERS)
  287.         {
  288.             walk1 = feelers();
  289.             if (walk1 != NO_FEELER_TOUCHED) return walk1;//Si hay alg·n sensor activo se sale
  290.         }
  291.         
  292.      }
  293.      return NO_FEELER_TOUCHED;
  294. }
  295.  
  296.  
  297. //**************************************************************************//
  298. //Procedimiento: walk_back()                                                        //
  299. //**************************************************************************//
  300. //Entradas:     nada
  301. //              
  302. //Devuelve:     Nada
  303. //
  304. //Nota:     Da un paso hacia atrßs
  305. //**************************************************************************//
  306. void walk_back()
  307. {
  308.     set_bit(PORTB,RIGHT_EYE);
  309.     set_bit(PORTB,LEFT_EYE);
  310.     for (counter2=1;counter2<=4;counter2++)
  311.     {
  312.         walk(Avanza_patas_izquierdas,Apoya_patas_izquierdas,Retrasa_patas_derechas,DO_NOT_CHECK_FEELERS);
  313.         walk(Retrasa_patas_izquierdas,Apoya_patas_derechas,Avanza_patas_derechas,DO_NOT_CHECK_FEELERS);
  314.     }
  315. }
  316.  
  317.  
  318. //**************************************************************************//
  319. //Procedimiento: calibrate()
  320. //**************************************************************************//
  321. //Entradas:     nada
  322. //              
  323. //Devuelve:     Nada
  324. //
  325. //Nota:         Pone los servos izquierdo y derecho e posici≤n central
  326. //**************************************************************************//
  327. void calibrate(void)
  328. {
  329.      
  330.      while(1){
  331.         pulse_out(RIGHT_SERVO_PIN,CENTER);
  332.         pulse_out(LEFT_SERVO_PIN,CENTER);
  333.         delay_ms(TIME_DELAY);
  334.      }
  335.      
  336. }
  337.  
  338.  
  339. //**************************************************************************//
  340. //Procedimiento: delay_dms
  341. //**************************************************************************//
  342. //Entradas:     duraci≤n del retardo en unidades de 10 us
  343. //              
  344. //Devuelve:     Nada
  345. //
  346. //Nota:         el retardo final es de duracion*10 us + 2 us + 2 us
  347. //**************************************************************************//
  348. void delay_dms(char duracion)
  349. {
  350.     asm{
  351.     
  352. bucle
  353.         nop
  354.         nop
  355.         nop
  356.         nop
  357.         nop
  358.         nop
  359.         nop
  360.         decfsz param00_delay_dms, f
  361.         goto bucle
  362.         return
  363.     }
  364.  
  365. }