Solarboat Twente / SOS_Stability_Controller

Controller YOLO

Dependencies:   Epos_control BIEM_Xbus CAN_IDs mbed-rtos mbed

Fork of SOS_Stability_Controller by Bram Seinhorst

Files at this revision

API Documentation at this revision

Comitter:
BramS23
Date:
Fri May 19 16:47:49 2017 +0000
Parent:
2:dd0e8ab74c7f
Child:
4:65589629dfd0
Commit message:
DONEZO;

Changed in this revision

BIEM_pinout.h Show annotated file Show diff for this revision Revisions of this file
CAN_IDs.h Show diff for this revision Revisions of this file
CAN_IDs.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/BIEM_pinout.h	Tue May 16 12:12:06 2017 +0000
+++ b/BIEM_pinout.h	Fri May 19 16:47:49 2017 +0000
@@ -32,15 +32,15 @@
  
  
 // CAN
-#define CAN_RD          PB_8
-#define CAN_TD          PB_9
+#define CAN_RD          PA_11
+#define CAN_TD          PA_12
  
  
 // XSENS
 #define XSENS_RX        PA_9
 #define XSENS_TX        PA_10
  
-#define XSENS_PING      PC_8
+#define XSENS_PING      PA_7
  
  
 // BIEM
--- a/CAN_IDs.h	Tue May 16 12:12:06 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,59 +0,0 @@
-#ifndef CANIDs
-#define CANIDs
-
-// ID list for can
-
-// BMS ID
-#define CANID_BMS = 0x033
-
-// 0x050 - 0x100 Communication between Nucleo IDs
-#define CANID_ToBiem_EnableDisable  0x051
-    // data     0x01    Enable all systems
-    //          0x02    Only enable Xsens
-    //          0x11    Disable all systems
-    //          0x12    Disable only EPOS
-    
-#define CANID_ToBiem_Button_press   0x051
-#define CANID_ToReserved            0x052
-//etc
-
-#define CANID_FromBiem              0x061
-    // data     0x01    Complete Setup Succesfull
-    //          0x02    Only Xsens Setup was succesfull
-    //          0x03    Setup unsuccesfull
-    //          0x11    Disable Succesfull
-    //          0x12    Disable Unsuccesfull
-                
-    
-#define CANID_FromReserved          0x062
-
-// 0x100 - 0x200 Sensor IDs
-#define CANID_Sens1                 0x101
-#define CANID_Sens2                 0x102
-
-#define CANID_Lat                   0x111   // float
-#define CANID_Long                  0x112   // float
-#define CANID_Vel                   0x113   // float
-#define CANID_ControllerMode        0x199   // int
-
-
-// 0x580 - 0x620 EPOS IDs
-#define CANID_ALLEPOS_SendTo        0x600
-#define CANID_EPOS1_SendTo          0x601
-#define CANID_EPOS2_SendTo          0x602
-#define CANID_EPOS3_SendTo          0x603
-#define CANID_EPOS4_SendTo          0x604
-#define CANID_EPOS5_SendTo          0x605
-
-#define CANID_ALLEPOS_RecieveFrom   0x600
-#define CANID_EPOS1_RecieveFrom     0x601
-#define CANID_EPOS2_RecieveFrom     0x602
-#define CANID_EPOS3_RecieveFrom     0x603
-#define CANID_EPOS4_RecieveFrom     0x604
-#define CANID_EPOS5_RecieveFrom     0x605
-
-
-
-
-
-#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN_IDs.lib	Fri May 19 16:47:49 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Solarboat-Twente/code/CAN_IDs/#7b9b18d10290
--- a/main.cpp	Tue May 16 12:12:06 2017 +0000
+++ b/main.cpp	Fri May 19 16:47:49 2017 +0000
@@ -7,7 +7,7 @@
 #include "SOS_ss.h"
 #include "CAN_IDs.h"
 #define DEBUG
-//#define NO_CAN_TEST
+#define NO_CAN_TEST
 //#define DEBUG_CANMSG
 //#define PRINTDATA
 
@@ -28,6 +28,7 @@
 Thread threadSysLOOP;
 Thread threadDATALOOP;
 Thread thread4;
+Thread thread5;
 
 // create EPOS CONTROLLERS
 EPOS epos1;
@@ -334,14 +335,18 @@
             }
             else if (ID==CANID_ToBiem_EnableDisable){
                 
-                threadSysLOOP.terminate();
-                threadDATALOOP.terminate();
-                thread4.terminate();
+                
                 
                 CANMessage Response;
                 Response.id=CANID_FromBiem;
                 Response.len=1;
-                if (msg.data[0] == 0x01){
+                Response.data[0]=0x99;
+                if ((msg.data[0] == 0x01)||(msg.data[0] == 0x02)){
+                    
+                    threadSysLOOP.terminate();
+                    threadDATALOOP.terminate();
+                    thread4.terminate();
+                    thread5.terminate();
                     
                     #ifdef DEBUG
                     pc.printf("ENABLE all Recieved \r\n");
@@ -361,11 +366,12 @@
                             #endif
                             Response.data[0]=0x01;
                             
-                            if (All_Epos2PosMode()){
+                            if ((msg.data[0] == 0x01)&&(All_Epos2PosMode())){
                                 #ifdef DEBUG
                                 pc.printf("POS Mode Set \r\nStarting Full Control Loop \r\n");
                                 #endif
                                 threadSysLOOP.start(&BIEM_Control_Sys_Loop);
+                                thread5.start(&Send_Can_Data);
                             }
                             else {
                                 #ifdef DEBUG
@@ -376,6 +382,7 @@
                                 pc.printf("Starting Reduced Control Loop \r\n");
                                 #endif
                                 threadSysLOOP.start(&BIEM_EPOS_Disabled_Loop);
+                                thread5.start(&Send_Can_Data);
                             }
                         }
                         else {
@@ -387,6 +394,7 @@
                             pc.printf("Starting Reduced Control Loop \r\n");
                             #endif
                             threadSysLOOP.start(&BIEM_EPOS_Disabled_Loop);
+                            thread5.start(&Send_Can_Data);
                         }
                         
                         #ifdef DEBUG
@@ -407,7 +415,14 @@
                     Response.data[0]=0x03;
                     }
                 }
-                else if (msg.data[0] == 0){
+                else if ((msg.data[0] == 0x11)||(msg.data[0] == 0x12)){
+                    
+                
+                    threadSysLOOP.terminate();
+                    threadDATALOOP.terminate();
+                    thread4.terminate();
+                    thread5.terminate();
+                    
                     
                     #ifdef DEBUG
                     pc.printf("DISABLE Recieved \r\n");
@@ -420,10 +435,16 @@
                     }
                     else{
                         #ifdef DEBUG
-                        pc.printf("Disable Succesfull \r\n");
+                        pc.printf("Disable Unsuccesfull \r\n");
                         #endif 
                         Response.data[0]=0x12;
                     }
+                    
+                    threadSysLOOP.start(&BIEM_EPOS_Disabled_Loop);
+                    #ifdef PRINTDATA
+                    threadDATALOOP.start(&Control_Data_Print_Loop);
+                    #endif
+                    thread4.start(&Send_Can_Data);
                 }
                 can.write(Response);
                 Thread::wait(10);
@@ -473,6 +494,7 @@
             Thread::wait(100);
             threadDATALOOP.start(&Control_Data_Print_Loop);
             #endif
+            thread5.start(&Send_Can_Data);
     #endif
 
     // osWaitForever