Diff: main.cpp

Revision:

26:2b865c00d7e9

Parent:

25:f5741040c4bb

Child:

27:501fee691e0e

--- a/main.cpp	Sun Apr 09 03:05:52 2017 +0000
+++ b/main.cpp	Mon May 01 15:22:58 2017 +0000
@@ -6,21 +6,28 @@
 
 #define REST_MODE 0
 #define CALIBRATION_MODE 1
-#define TORQUE_MODE 2
-#define PD_MODE 3
+#define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
+#define P_MASK
+#define D_MASK
+#define KP_MASK
+#define KD_MASK
+#define TFF_MASK
 
+/*
 const unsigned int BOARDNUM = 0x2;
 //const unsigned int a_id =                            
-const unsigned int TX_ID = 0x0100;
 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
+*/
+const unsigned int TX_ID = 0x01;
+
 
-float __float_reg[64];
-int __int_reg[256];
+float __float_reg[64];                                                          // Floats stored in flash
+int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
-#include "CANnucleo.h"
+
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -36,6 +43,7 @@
 #include "user_config.h"
 #include "PreferenceWriter.h"
 
+ 
 PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
@@ -44,70 +52,112 @@
 VelocityEstimatorStruct velocity;
 
 
-CANnucleo::CAN          can(PB_8, PB_9);                                        // CAN Rx pin name, CAN Tx pin name
-CANnucleo::CANMessage   rxMsg;
-CANnucleo::CANMessage   txMsg;
-int                     ledState;
-int                     counter = 0;
-int canCmd = 1000;
-volatile bool           msgAvailable = false;
-
-DigitalOut toggle(PA_0);
-Ticker  loop;
-/**
- * @brief   'CAN receive-complete' interrup handler.
- * @note    Called on arrival of new CAN message.
- *          Keep it as short as possible.
- * @param   
- * @retval  
- */
-void onMsgReceived() {
-    msgAvailable = true;
-    //printf("ping\n\r");
-}
+//using namespace CANnucleo;
 
-void sendCMD(int TX_addr, int val){
-    txMsg.clear();      //clear Tx message storage
-    txMsg.id = TX_addr;
-    txMsg << val;
-    can.write(txMsg);
-    //wait(.1);
-    
-    }
-    
-void readCAN(void){
-    if(msgAvailable) { 
-    msgAvailable = false;                                                       // reset flag for next use
-    can.read(rxMsg);                                                            // read message into Rx message storage
-    // Filtering performed by software:           
-    if(rxMsg.id == cmd_ID) {                                                    // See comments in CAN.cpp for filtering performed by hardware
-            rxMsg >> canCmd;                                                    // extract first data item
-            }                                                                   
-        }
-    }
-    
-void cancontroller(void){
-    //printf("%d\n\r", canCmd);
-    readCAN();
-    //sendCMD(TX_ID, canCmd);
+CAN          can(PB_8, PB_9);                                        // CAN Rx pin name, CAN Tx pin name
+CANMessage   rxMsg;
+CANMessage   txMsg;
 
-    }
-    
 
 Serial pc(PA_2, PA_3);
 
-PositionSensorAM5147 spi(16384, 0.0, NPP);   
-PositionSensorEncoder encoder(4096, 0, 21); 
+PositionSensorAM5147 spi(16384, 0.0, NPP);  
+PositionSensorEncoder encoder(4096, 0, NPP); 
+
+
+DigitalOut toggle(PA_0);
 
 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 -30.0f
+ #define V_MAX 30.0f
+ #define KP_MIN 0.0f
+ #define KP_MAX 500.0f
+ #define KD_MIN 0.0f
+ #define KD_MAX 100.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 i){
+    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 i_int = float_to_uint(i, -I_MAX, I_MAX, 12);
+    msg->data[0] = p_int>>8;
+    msg->data[1] = p_int&0xFF;
+    msg->data[2] = v_int>>4;
+    msg->data[3] = ((v_int&0xF)<<4) + (i_int>>8);
+    msg->data[4] = i_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);
+    can.read(rxMsg);  
+    if((rxMsg.id == CAN_ID) && (state == MOTOR_MODE)){
+        unpack_cmd(rxMsg, &controller);
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q);
+        can.write(txMsg);
+    }
+    
+}
+
 void enter_menu_state(void){
     printf("\n\r\n\r\n\r");
     printf(" Commands:\n\r");
-    printf(" t - Torque Mode\n\r");
-    printf(" p - PD Mode\n\r");
+    printf(" m - Motor Mode\n\r");
     printf(" c - Calibrate Encoder\n\r");
     printf(" s - Setup\n\r");
     printf(" e - Display Encoder\n\r");
@@ -121,6 +171,7 @@
     printf(" %-7s %-25s %-5s %-5s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
     printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
     printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+    printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
     printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
     printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
     state_change = 0;
@@ -128,19 +179,19 @@
     
 void enter_torque_mode(void){
     controller.i_d_ref = 0;
-    controller.i_q_ref = 1;                                                     // Current Setpoints
+    controller.i_q_ref = 6;                                                     // Current Setpoints
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     gpio.enable->write(1);                                                      // Enable gate drive
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     state_change = 0;
     }
     
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
-    GPIOC->ODR ^= (1 << 5);                                                     // Turn off status LED
+    GPIOC->ODR &= !(1 << 5);                                                     // Turn off status LED
     wait(.2);
     gpio.enable->write(0);                                                      // Turn off gate drive
     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
@@ -184,24 +235,31 @@
                     }
                 break;
              
-            case TORQUE_MODE:                                                   // Run torque control
+            case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
                     enter_torque_mode();
                     }
                 count++;
-                controller.theta_elec = spi.GetElecPosition();                  
+                toggle.write(1);
+                controller.theta_elec = spi.GetElecPosition();
+                controller.theta_mech = spi.GetMechPosition();
+                controller.dtheta_mech = spi.GetMechVelocity();  
+                //TIM1->CCR3 = 0x708*(1.0f);
+                //TIM1->CCR1 = 0x708*(1.0f);
+                //TIM1->CCR2 = 0x708*(1.0f);     
+                
+                //controller.i_q_ref = controller.t_ff/KT_OUT;   
+                //torque_control(&controller);      
+                controller.i_q_ref = 1; 
                 commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                 spi.Sample();                                                   // Sample position sensor
+                toggle.write(0);
+                
                 if(count > 100){
                      count = 0;
-                     //readCAN();
-                     //controller.i_q_ref = ((float)(canCmd-1000))/100;
-                    //pc.printf("%f\n\r ", controller.theta_elec);
+                     //printf("%d  %d\n\r", controller.adc1_raw, controller.adc2_raw);
                      }
                 break;
-            
-            case PD_MODE:
-                break;
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();
@@ -211,9 +269,7 @@
                 print_encoder();
                 break;
                 }
-                
-   
-            
+                 
       }
   TIM1->SR = 0x0;                                                               // reset the status register
 }
@@ -233,6 +289,7 @@
                 state_change = 1;
                 char_count = 0;
                 cmd_id = 0;
+                GPIOC->ODR &= !(1 << 5); 
                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                 }
         if(state == REST_MODE){
@@ -241,8 +298,8 @@
                     state = CALIBRATION_MODE;
                     state_change = 1;
                     break;
-                case 't':
-                    state = TORQUE_MODE;
+                case 'm':
+                    state = MOTOR_MODE;
                     state_change = 1;
                     break;
                 case 'e':
@@ -264,6 +321,9 @@
                     case 'i':
                         CAN_ID = atoi(cmd_val);
                         break;
+                    case 'm':
+                        CAN_MASTER = atoi(cmd_val);
+                        break;
                     case 'l':
                         TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
                         break;
@@ -310,24 +370,27 @@
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
-    //TIM1->CR1 |= TIM_CR1_UDIS;
-    gpio.enable->write(1);                                                      // Enable gate drive
-    gpio.pwm_u->write(1.0f);                                                    // Write duty cycles
-    gpio.pwm_v->write(1.0f);
-    gpio.pwm_w->write(1.0f);
+    gpio.enable->write(1);
+    TIM1->CCR3 = 0x708*(1.0f);                        // Write duty cycles
+    TIM1->CCR2 = 0x708*(1.0f);
+    TIM1->CCR1 = 0x708*(1.0f);
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
-    //gpio.enable->write(0);
+    gpio.enable->write(0);
     reset_foc(&controller);                                                     // Reset current controller
-    
-    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    TIM1->CR1 ^= TIM_CR1_UDIS;
+    //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
     wait(.1);
     NVIC_SetPriority(TIM5_IRQn, 2);                                             // set interrupt priority
 
+
     can.frequency(1000000);                                                     // set bit rate to 1Mbps
-    can.attach(&onMsgReceived);                                                 // attach 'CAN receive-complete' interrupt handler
-    can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
-    
+    can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    //can.filter(CAN_ID, 0xF, CANStandard, 0);
+    can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler                                                                    
+    txMsg.id = TX_ID;
+    txMsg.len = 5;
+    rxMsg.len = 8;
     
     prefs.load();                                                               // Read flash
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
@@ -335,7 +398,7 @@
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     
-    pc.baud(115200);                                                            // set serial baud rate
+    pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
     wait(.01);