Revision 35:69b24894c11d, committed 2017-11-18

Comitter:

benkatz

Date:

Sat Nov 18 18:41:42 2017 +0000

Parent:

34:51647c6c500d

Child:

36:d88fd41f60a6

Commit message:

minor updates

Changed in this revision

--- a/Config/current_controller_config.h	Wed Nov 08 15:18:18 2017 +0000
+++ b/Config/current_controller_config.h	Sat Nov 18 18:41:42 2017 +0000
@@ -3,7 +3,7 @@
 
 #define K_D .05f                     // Volts/Amp
 #define K_Q .05f                     // Volts/Amp
-#define K_SCALE 0.00043f            // K_loop/Loop BW (Hz)
+#define K_SCALE 0.0001f            // K_loop/Loop BW (Hz) 0.0042
 #define KI_D 0.0255f                  // 1/samples
 #define KI_Q 0.0255f                  // 1/samples
 #define V_BUS 24.0f                 // Volts

--- a/FOC/foc.cpp	Wed Nov 08 15:18:18 2017 +0000
+++ b/FOC/foc.cpp	Sat Nov 18 18:41:42 2017 +0000
@@ -97,7 +97,7 @@
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_q_error = controller->i_q_ref - controller->i_q + cogging_current - 2.0f;
+       float i_q_error = controller->i_q_ref - controller->i_q + cogging_current;
        float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE);   //feed-forward voltage
        float v_q_ff =  controller->dtheta_elec*WB*1.73205081f;
        controller->d_int += i_d_error;   

--- a/PositionSensor/PositionSensor.cpp	Wed Nov 08 15:18:18 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sat Nov 18 18:41:42 2017 +0000
@@ -63,7 +63,7 @@
         vel = (modPosition-oldModPosition)*40000.0f;
     }    
     
-    int n = 16;
+    int n = 40;
     float sum = vel;
     for (int i = 1; i < (n); i++){
         velVec[n - i] = velVec[n-i-1];

--- a/PositionSensor/PositionSensor.h	Wed Nov 08 15:18:18 2017 +0000
+++ b/PositionSensor/PositionSensor.h	Sat Nov 18 18:41:42 2017 +0000
@@ -36,7 +36,7 @@
     virtual void ZeroEncoderCountDown(void);
     int _CPR, flag, rotations, _ppairs, raw;
     //int state;
-    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[16];
+    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
     int offset_lut[128];
 };
 
@@ -53,7 +53,7 @@
     virtual int GetCPR(void);
     virtual void WriteLUT(int new_lut[128]);
 private:
-    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[16], MechVelocity, ElecVelocity, ElecVelocityFilt;
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
     int raw, _CPR, rotations, old_counts, _ppairs;
     SPI *spi;
     DigitalOut *cs;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Sat Nov 18 18:41:42 2017 +0000
@@ -0,0 +1,442 @@
+/// high-bandwidth 3-phase motor control, for robots
+/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
+/// Hardware documentation can be found at build-its.blogspot.com
+
+/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
+
+#define REST_MODE 0
+#define CALIBRATION_MODE 1
+#define MOTOR_MODE 2
+#define SETUP_MODE 4
+#define ENCODER_MODE 5
+
+#define VERSION_NUM "1.0.1"
+
+
+float __float_reg[64];                                                          // Floats stored in flash
+int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+
+
+#include "mbed.h"
+#include "PositionSensor.h"
+#include "structs.h"
+#include "foc.h"
+#include "calibration.h"
+#include "hw_setup.h"
+#include "math_ops.h" 
+#include "current_controller_config.h"
+#include "hw_config.h"
+#include "motor_config.h"
+#include "stm32f4xx_flash.h"
+#include "FlashWriter.h"
+#include "user_config.h"
+#include "PreferenceWriter.h"
+
+ 
+PreferenceWriter prefs(6);
+
+GPIOStruct gpio;
+ControllerStruct controller;
+COMStruct com;
+VelocityEstimatorStruct velocity;
+
+
+//using namespace CANnucleo;
+
+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, 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 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 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] = 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) + (i_int>>8);
+    msg->data[5] = 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)){
+        controller.timeout = 0;
+        if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){
+            state = MOTOR_MODE;
+            state_change = 1;
+            }
+        else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
+            state = REST_MODE;
+            state_change = 1;
+            GPIOC->ODR &= !(1 << 5); 
+            }
+        else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
+            spi.ZeroPosition();
+            }
+        else if(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(" m - Motor Mode\n\r");
+    printf(" c - Calibrate Encoder\n\r");
+    printf(" s - Setup\n\r");
+    printf(" e - Display Encoder\n\r");
+    printf(" esc - Exit to Menu\n\r");
+    state_change = 0;
+    gpio.enable->write(0);
+    }
+
+void enter_setup_state(void){
+    printf("\n\r\n\r Configuration Options \n\r\n\n");
+    printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
+    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
+    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
+    printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
+    printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
+    state_change = 0;
+    }
+    
+void enter_torque_mode(void){
+    gpio.enable->write(1);                                                      // Enable gate drive
+    reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
+    wait(.001);
+    controller.i_d_ref = 0;
+    controller.i_q_ref = 0;                                                     // Current Setpoints
+    GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
+    state_change = 0;
+    printf("\n\r Entering Motor Mode \n\r");
+    }
+    
+void calibrate(void){
+    gpio.enable->write(1);                                                      // Enable gate drive
+    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
+    wait(.2);
+    gpio.enable->write(0);                                                      // Turn off gate drive
+    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
+     state_change = 0;
+     
+    }
+    
+void print_encoder(void){
+    spi.Sample();
+    wait(.001);
+    printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+    wait(.05);
+    }
+
+/// Current Sampling Interrupt ///
+/// This runs at 40 kHz, regardless of of the mode the controller is in ///
+extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
+  if (TIM1->SR & TIM_SR_UIF ) {
+        //toggle = 1;
+
+        ///Sample current always ///
+        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
+        //volatile int delay;   
+        //for (delay = 0; delay < 55; delay++);
+        controller.adc2_raw = ADC2->DR;                                         // Read ADC1 and ADC2 Data Registers
+        controller.adc1_raw = ADC1->DR;
+        ///
+        
+        /// Check state machine state, and run the appropriate function ///
+        //printf("%d\n\r", state);
+        switch(state){
+            case REST_MODE:                                                     // Do nothing until
+                if(state_change){
+                    enter_menu_state();
+                    }
+                break;
+            
+            case CALIBRATION_MODE:                                              // Run encoder calibration procedure
+                if(state_change){
+                    calibrate();
+                    }
+                break;
+             
+            case MOTOR_MODE:                                                   // Run torque control
+                if(state_change){
+                    enter_torque_mode();
+                    count = 0;
+                    }
+                else{
+                count++;
+                //toggle.write(1);
+                controller.theta_elec = spi.GetElecPosition();
+                controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
+                controller.dtheta_mech = (1.0f/GR)*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);     
+                if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
+                    controller.i_d_ref = 0;
+                    controller.i_q_ref = 0;
+                    } 
+                //controller.i_q_ref = .5; 
+                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
+                spi.Sample();                                                   // Sample position sensor
+                //toggle.write(0);
+                controller.timeout += 1;
+                
+                if(count == 4000){
+                     count = 0;
+                     //wait(.001);
+                    //printf(" %.5f  \n\r", controller.theta_mech);
+                     }
+                     }
+                     
+                break;
+            case SETUP_MODE:
+                if(state_change){
+                    enter_setup_state();
+                }
+                break;
+            case ENCODER_MODE:
+                print_encoder();
+                break;
+                }
+                 
+      }
+  TIM1->SR = 0x0;                                                               // reset the status register
+}
+
+
+char cmd_val[8] = {0};
+char cmd_id = 0;
+char char_count = 0;
+
+/// Manage state machine with commands from serial terminal or configurator gui ///
+/// Called when data received over serial ///
+void serial_interrupt(void){
+    while(pc.readable()){
+        char c = pc.getc();
+        if(c == 27){
+                state = REST_MODE;
+                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){
+            switch (c){
+                case 'c':
+                    state = CALIBRATION_MODE;
+                    state_change = 1;
+                    break;
+                case 'm':
+                    state = MOTOR_MODE;
+                    state_change = 1;
+                    break;
+                case 'e':
+                    state = ENCODER_MODE;
+                    state_change = 1;
+                    break;
+                case 's':
+                    state = SETUP_MODE;
+                    state_change = 1;
+                    break;
+                    }
+                }
+        else if(state == SETUP_MODE){
+            if(c == 13){
+                switch (cmd_id){
+                    case 'b':
+                        I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
+                        break;
+                    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;
+                    case 't':
+                        CAN_TIMEOUT = atoi(cmd_val);
+                        break;
+                    default:
+                        printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
+                        break;
+                    }
+                    
+                if (!prefs.ready()) prefs.open();
+                prefs.flush();                                                  // Write new prefs to flash
+                prefs.close();    
+                prefs.load();                                              
+                state_change = 1;
+                char_count = 0;
+                cmd_id = 0;
+                for(int i = 0; i<8; i++){cmd_val[i] = 0;}
+                }
+            else{
+                if(char_count == 0){cmd_id = c;}
+                else{
+                    cmd_val[char_count-1] = c;
+                    
+                }
+                pc.putc(c);
+                char_count++;
+                }
+            }
+        else if (state == ENCODER_MODE){
+            switch (c){
+                case 27:
+                    state = REST_MODE;
+                    state_change = 1;
+                    break;
+                    }
+            }
+            
+        }
+    }
+       
+int main() {
+    
+    controller.v_bus = V_BUS;
+    controller.mode = 0;
+    Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
+
+    wait(.1);
+    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);
+    reset_foc(&controller);                                                     // Reset current controller
+    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.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 = CAN_MASTER;
+    txMsg.len = 6;
+    rxMsg.len = 8;
+    
+    prefs.load();                                                               // Read flash
+    spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
+    int lut[128] = {0};
+    memcpy(&lut, &ENCODER_LUT, sizeof(lut));
+    spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
+    
+    pc.baud(921600);                                                            // set serial baud rate
+    wait(.01);
+    pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+    wait(.01);
+    printf("\n\r Debug Info:\n\r");
+    printf(" Firmware Version: %s\n\r", VERSION_NUM);
+    printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
+    printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
+    printf(" CAN ID:  %d\n\r", CAN_ID);
+        
+    pc.attach(&serial_interrupt);                                               // attach serial interrupt
+    
+    state_change = 1;
+
+    
+    while(1) {
+
+    }
+}