Merge branch 'master' of ssh://rtime.felk.cvut.cz/eurobot

diff --git a/src/eb_jaws_11/main.c b/src/eb_jaws_11/main.c
index 68eea98b4638b6d14465ca404e3fe34bb80960d2..9b6ef55aaef8f8d2554677093e3d816e6833beb6 100644 (file)
--- a/src/eb_jaws_11/main.c
+++ b/src/eb_jaws_11/main.c
@@ -33,12 +33,16 @@
 #include "fsm.h"
 #include "def.h"
 #include <adc.h>
+#include <servo.h>
 
 #define        CAN_SPEED       1000000         //< CAN bus speed
 #define CAN_ISR                0
 
 #define ADC_ISR                1
 
+#define JAW_LEFT       0
+#define JAW_RIGHT      1
+
 #define TIMER_IRQ_PRIORITY     5
 
 
@@ -120,19 +124,32 @@ void CAN_rx(can_msg_t *msg) {
        deb_led_on(LEDB);
 
        switch (rx_msg.id) 
-       {               
+       {       
+               //JAW_LEFT_OPEN ff      0xff
+               //JAW_RIGHT_OPEN        0x00
+               
+               //JAW_LEFT_CLOSE        0x80
+               //JAW_RIGHT_CLOSE       0x80
+               
+               //JAW_LEFT_CATCH        0xB8    
+               //JAW_RIGHT_CATCH       0x38
+               
                case CAN_JAW_LEFT_CMD:
-                
+                         
                        deb_led_on(LEDB);
                        req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
                        spd = rx_msg.data[2];
+                                               
+                       if (req >= 0x80 && req <= 0xff) {
+                               set_servo(JAW_LEFT, (char)req);
+                       }
                        
-                       if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
-                               fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
-                               fsm_jaw_left.can_req_position = req;
-                               fsm_jaw_left.can_req_spd = spd;
-                       } else
-                               fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
+//                     if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
+//                             fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
+//                             fsm_jaw_left.can_req_position = req;
+//                             fsm_jaw_left.can_req_spd = spd;
+//                     } else
+//                             fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
                break;
                
                case CAN_JAW_RIGHT_CMD:
@@ -140,14 +157,19 @@ void CAN_rx(can_msg_t *msg) {
                        deb_led_on(LEDB);
                        req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
                        spd = rx_msg.data[2];
-               
-                       if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
-                               fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
-                               fsm_jaw_right.can_req_position = req;
-                               fsm_jaw_right.can_req_spd = spd;
-                       } else
-                               fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
+                       
+                       if (req >= 0x00 && req <= 0x80) {
+                               set_servo(JAW_RIGHT, (char)req);
+                       }
+                       
+//                     if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
+//                             fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
+//                             fsm_jaw_right.can_req_position = req;
+//                             fsm_jaw_right.can_req_spd = spd;
+//                     } else
+//                             fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
                break;
+               
                default:break;
        }
 
@@ -168,8 +190,11 @@ void init_periphery(void){
        init_uart();
        init_adc(ADC_ISR);
        
-       
-} 
+       init_servo(7);  //7 is interrupt priority
+       set_servo(JAW_LEFT, 0x80);
+       set_servo(JAW_RIGHT,0x80);
+}
+
 /*********************************************************/
 void can_send_status(struct fsm *fsm){
        can_msg_t tx_msg;