Revision 42:738fa01b0346, committed 2018-05-25

Comitter:

benkatz

Date:

Fri May 25 15:16:42 2018 +0000

Parent:

41:304ac2affb23

Child:

43:dfb72608639c

Commit message:

separated CAN stuff out of main

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN_com.cpp	Fri May 25 15:16:42 2018 +0000
@@ -0,0 +1,71 @@
+#include "CAN_com.h"
+
+
+ #define P_MIN -12.5f
+ #define P_MAX 12.5f
+ #define V_MIN -45.0f
+ #define V_MAX 45.0f
+ #define KP_MIN 0.0f
+ #define KP_MAX 500.0f
+ #define KD_MIN 0.0f
+ #define KD_MAX 5.0f
+ #define T_MIN -18.0f
+ #define T_MAX 18.0f
+ 
+
+/// CAN Reply Packet Structure ///
+/// 16 bit position, between -4*pi and 4*pi
+/// 12 bit velocity, between -30 and + 30 rad/s
+/// 12 bit current, between -40 and 40;
+/// CAN Packet is 5 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], current[11-8]]
+/// 4: [current[7-0]]
+void pack_reply(CANMessage *msg, float p, float v, float t){
+    int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
+    int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
+    int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
+    msg->data[0] = CAN_ID;
+    msg->data[1] = p_int>>8;
+    msg->data[2] = p_int&0xFF;
+    msg->data[3] = v_int>>4;
+    msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
+    msg->data[5] = t_int&0xFF;
+    }
+    
+/// CAN Command Packet Structure ///
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void unpack_cmd(CANMessage msg, ControllerStruct * controller){
+        int p_int = (msg.data[0]<<8)|msg.data[1];
+        int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
+        int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
+        int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
+        int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
+        
+        controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
+        controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
+        controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
+        controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
+        controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
+    //printf("Received   ");
+    //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
+    //printf("\n\r");
+    }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN_com.h	Fri May 25 15:16:42 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef CAN_COM_H
+#define CAN_COM_H
+
+#include "structs.h"
+#include "user_config.h"
+#include "mbed.h"
+#include "math_ops.h"
+
+void pack_reply(CANMessage *msg, float p, float v, float t);
+void unpack_cmd(CANMessage msg, ControllerStruct * controller);
+
+
+#endif
\ No newline at end of file

--- a/loops.cpp	Thu May 24 15:03:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,10 +0,0 @@
-#include "loops.h"
-#include "foc.h"
-
-void pd1(float p_des, float v_des, float kp, float kd){
-    
-    }
-void pd2(float p_des, float kp, float kd);
-
-void torque(float t){
-    }
\ No newline at end of file

--- a/loops.h	Thu May 24 15:03:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,12 +0,0 @@
-#ifndef LOOPS_H
-#define LOOPS_H
-
-#include "structs.h"
-#include "PositionSensor.h"
-
-
-void pd1(float p_des, float v_des, float kp, float kd);
-void pd2(float p_des, float kp, float kd);
-void torque(float t);
-
-#endif

--- a/main.cpp	Thu May 24 15:03:25 2018 +0000
+++ b/main.cpp	Fri May 25 15:16:42 2018 +0000
@@ -30,7 +30,7 @@
 #include "FlashWriter.h"
 #include "user_config.h"
 #include "PreferenceWriter.h"
-
+#include "CAN_com.h"
 
  
 PreferenceWriter prefs(6);
@@ -40,9 +40,8 @@
 COMStruct com;
 ObserverStruct observer;
 
-//using namespace CANnucleo;
 
-CAN          can(PB_8, PB_9);                                        // CAN Rx pin name, CAN Tx pin name
+CAN          can(PB_8, PB_9);      // CAN Rx pin name, CAN Tx pin name
 CANMessage   rxMsg;
 CANMessage   txMsg;
 
@@ -50,83 +49,11 @@
 Serial pc(PA_2, PA_3);
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);  
-//PositionSensorEncoder encoder(4096, 0, NPP); 
-
-
-DigitalOut toggle(PC_8);
 
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
 
- #define P_MIN -12.5f
- #define P_MAX 12.5f
- #define V_MIN -45.0f
- #define V_MAX 45.0f
- #define KP_MIN 0.0f
- #define KP_MAX 500.0f
- #define KD_MIN 0.0f
- #define KD_MAX 5.0f
- #define T_MIN -18.0f
- #define T_MAX 18.0f
- 
-
-/// CAN Reply Packet Structure ///
-/// 16 bit position, between -4*pi and 4*pi
-/// 12 bit velocity, between -30 and + 30 rad/s
-/// 12 bit current, between -40 and 40;
-/// CAN Packet is 5 8-bit words
-/// Formatted as follows.  For each quantity, bit 0 is LSB
-/// 0: [position[15-8]]
-/// 1: [position[7-0]] 
-/// 2: [velocity[11-4]]
-/// 3: [velocity[3-0], current[11-8]]
-/// 4: [current[7-0]]
-void pack_reply(CANMessage *msg, float p, float v, float t){
-    int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
-    int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
-    int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
-    msg->data[0] = CAN_ID;
-    msg->data[1] = p_int>>8;
-    msg->data[2] = p_int&0xFF;
-    msg->data[3] = v_int>>4;
-    msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
-    msg->data[5] = t_int&0xFF;
-    }
-    
-/// CAN Command Packet Structure ///
-/// 16 bit position command, between -4*pi and 4*pi
-/// 12 bit velocity command, between -30 and + 30 rad/s
-/// 12 bit kp, between 0 and 500 N-m/rad
-/// 12 bit kd, between 0 and 100 N-m*s/rad
-/// 12 bit feed forward torque, between -18 and 18 N-m
-/// CAN Packet is 8 8-bit words
-/// Formatted as follows.  For each quantity, bit 0 is LSB
-/// 0: [position[15-8]]
-/// 1: [position[7-0]] 
-/// 2: [velocity[11-4]]
-/// 3: [velocity[3-0], kp[11-8]]
-/// 4: [kp[7-0]]
-/// 5: [kd[11-4]]
-/// 6: [kd[3-0], torque[11-8]]
-/// 7: [torque[7-0]]
-void unpack_cmd(CANMessage msg, ControllerStruct * controller){
-        int p_int = (msg.data[0]<<8)|msg.data[1];
-        int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
-        int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
-        int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
-        int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
-        
-        controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
-        controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
-        controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
-        controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
-        controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
-    //printf("Received   ");
-    //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
-    //printf("\n\r");
-    }
-
 void onMsgReceived() {
     //msgAvailable = true;
     //printf("%.3f   %.3f   %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q);