eb_jaws: repair send can status of right jaw

diff --git a/src/eb_jaws_11/fsm.h b/src/eb_jaws_11/fsm.h
index a50f2c973ac312f1d2450649b4ed02c611e7d244..5bb3ba5c1c0e6558e2e0a5184dad259a31ad2d2f 100644 (file)
--- a/src/eb_jaws_11/fsm.h
+++ b/src/eb_jaws_11/fsm.h
@@ -47,6 +47,7 @@ struct fsm {
        engine_fcn engine_en;
        engine_fcn engine_dir;
        engine_fcn engine_pwm;
+        uint32_t can_id;
 //     enum motor motor;
 //     enum adc adc;
 };

diff --git a/src/eb_jaws_11/main.c b/src/eb_jaws_11/main.c
index 420b46976633dd0a298738159c42d2b658dfea19..e6dbfbc2fcdf9e86de62dedf47ba6584dc5e7fcc 100644 (file)
--- a/src/eb_jaws_11/main.c
+++ b/src/eb_jaws_11/main.c
@@ -129,8 +129,8 @@ void CAN_rx(can_msg_t *msg) {
                        
                        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;// save new req position of lift
-                               fsm_jaw_left.can_req_spd = spd;// save new req spd of lift
+                               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;
@@ -143,8 +143,8 @@ void CAN_rx(can_msg_t *msg) {
                
                        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;// save new req position of lift
-                               fsm_jaw_right.can_req_spd = spd;// save new req spd of lift
+                               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;
@@ -171,29 +171,18 @@ void init_periphery(void){
        
 } 
 /*********************************************************/
-void can_send_status(void)
-{
+void can_send_status(struct fsm *fsm){
        can_msg_t tx_msg;
-       tx_msg.id = CAN_JAW_LEFT_STATUS;
+       tx_msg.id = fsm->can_id;
        tx_msg.dlc = 5;
        tx_msg.flags = 0;
-       tx_msg.data[0] = (fsm_jaw_left.act_pos  >> 8) & 0xFF;
-       tx_msg.data[1] = fsm_jaw_left.act_pos & 0xFF;
-       tx_msg.data[2] = (fsm_jaw_left.can_response  >> 8) & 0xFF;
-       tx_msg.data[3] = fsm_jaw_left.can_response & 0xFF;
-       tx_msg.data[4] = fsm_jaw_left.flags; 
+       tx_msg.data[0] = (fsm->act_pos  >> 8) & 0xFF;
+       tx_msg.data[1] = fsm->act_pos & 0xFF;
+       tx_msg.data[2] = (fsm->can_response  >> 8) & 0xFF;
+       tx_msg.data[3] = fsm->can_response & 0xFF;
+       tx_msg.data[4] = fsm->flags; 
        /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
-       
-       tx_msg.id = CAN_JAW_RIGHT_STATUS;
-       tx_msg.dlc = 5;
-       tx_msg.flags = 0;
-       tx_msg.data[0] = (fsm_jaw_right.act_pos  >> 8) & 0xFF;
-       tx_msg.data[1] = fsm_jaw_right.act_pos & 0xFF;
-       tx_msg.data[2] = (fsm_jaw_right.can_response  >> 8) & 0xFF;
-       tx_msg.data[3] = fsm_jaw_right.can_response & 0xFF;
-       tx_msg.data[4] = fsm_jaw_right.flags; 
-       /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
-
+        
 }
 
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -344,12 +333,13 @@ void handle_bumper()
 int main(void)
 {
        uint32_t main_time = timer_usec;
-       uint32_t status_time = timer_usec;
+       uint32_t status_time_left = timer_usec;
+        uint32_t status_time_right = timer_usec;
        
 
        init_periphery();
                
-       SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
+       SET_PIN(PINSEL1, 1, PINSEL_0);
        SET_PIN(PINSEL1, 2, PINSEL_0);
        SET_PIN(PINSEL1, 3, PINSEL_0);
        SET_PIN(PINSEL0, BUMPER_REAR, PINSEL_0);
@@ -375,11 +365,15 @@ int main(void)
        fsm_jaw_left.engine_en = &engine_A_en;  
        fsm_jaw_left.engine_dir = &engine_A_dir;
        fsm_jaw_left.engine_pwm = &engine_A_pwm;
+        
+        fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
        
        //right jaw engine is engine B, conector MOTB on board
        fsm_jaw_right.engine_en = &engine_B_en; 
        fsm_jaw_right.engine_dir = &engine_B_dir;
        fsm_jaw_right.engine_pwm = &engine_B_pwm;
+        
+        fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
        
        init_fsm(&fsm_jaw_right, &fsm_jaw_init);
        init_fsm(&fsm_jaw_left, &fsm_jaw_init);
@@ -402,17 +396,22 @@ int main(void)
                  
                }
 
-               if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
-                   fsm_jaw_left.trigger_can_send || //repeat sending message every 100 ms
-                   fsm_jaw_right.trigger_can_send) {   //or when something important happen
+               if (timer_msec >= status_time_left + 100 || //repeat sending message every 100 ms
+                   fsm_jaw_left.trigger_can_send) {   //or when something important happen
                        fsm_jaw_left.trigger_can_send = false;
-                       fsm_jaw_right.trigger_can_send = false;
-                       status_time = timer_msec; //save new time, when message was sent
-                       can_send_status();
+                       status_time_left = timer_msec; //save new time, when message was sent
+                       can_send_status(&fsm_jaw_left);
 
                }
-
-//             start_button();
+                
+                if (timer_msec >= status_time_right + 100 || //repeat sending message every 100 ms
+                    fsm_jaw_right.trigger_can_send) {   //or when something important happen
+                        fsm_jaw_right.trigger_can_send = false;
+                        status_time_right = timer_msec; //save new time, when message was sent
+                        can_send_status(&fsm_jaw_right);
+
+                }
+                
                handle_bumper();
 
                blink_led();