Revision 54:3e056b097c52, committed 2020-03-03

Comitter:

bdring

Date:

Tue Mar 03 00:53:49 2020 +0000

Parent:

53:349304b6d937

Commit message:

Working on step/dir

Changed in this revision

--- a/Config/motor_config.h	Fri Feb 28 23:05:58 2020 +0000
+++ b/Config/motor_config.h	Tue Mar 03 00:53:49 2020 +0000
@@ -12,9 +12,14 @@
 #define R_TH 1.25f              //Kelvin per watt
 #define INV_M_TH 0.03125f       //Kelvin per joule
 
-#define P_MIN -12.5f // posiiton min in radians
-#define P_MAX 12.5f  // posiiton max in radians
+//#define P_MIN -12.5f // posiiton min in radians
+//#define P_MAX 12.5f  // posiiton max in radians
 
+#define P_MIN (-2 * PI) // posiiton min in radians
+#define P_MAX (2 * PI)  // posiiton max in radians
+
+#define STEPS_PER_REV 1000.0f  // ster per revolution
+#define RADS_PER_STEP ( (2.0f * PI) / STEPS_PER_REV) // radian move per step
 
 
 #endif

--- a/Config/user_config.h	Fri Feb 28 23:05:58 2020 +0000
+++ b/Config/user_config.h	Tue Mar 03 00:53:49 2020 +0000
@@ -18,8 +18,7 @@
 #define CAN_MASTER              __int_reg[2]                                    // CAN bus "master" ID
 #define CAN_TIMEOUT             __int_reg[3]                                    // CAN bus timeout period
 #define ENCODER_LUT             __int_reg[5]                                    // Encoder offset LUT - 128 elements long
-
-
+//#define IO_MODE                 __int_reg[6]                                    // The IO_mode (UART or step/dir)
 
 
 extern float __float_reg[];

--- a/main.cpp	Fri Feb 28 23:05:58 2020 +0000
+++ b/main.cpp	Tue Mar 03 00:53:49 2020 +0000
@@ -10,11 +10,19 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.9.1"
+#define VERSION_NUM "2.0.0"
 
+// this sets up the 2 IO modes of the UART connector (UART and Step/Dir)
+#define IO_MODE_NONE 0
+#define IO_MODE_SERIAL 1
+#define IO_MODE_STEP_DIR 2
+#define UART_TX PA_2 // define the pins on the connector
+#define UART_RX PA_3 // define the pins on the connector
+#define UART_BAUD 230400
+int io_mode = IO_MODE_NONE; // the default mode is serial
 
 float __float_reg[64];                                                          // Floats stored in flash
-int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table                                                                                
 
 #include "mbed.h"
 #include "PositionSensor.h"
@@ -39,17 +47,19 @@
 ControllerStruct controller;
 ObserverStruct observer;
 COMStruct com;
-Serial pc(PA_2, PA_3);
+
+
+Serial *pc = NULL;
+InterruptIn *step = NULL;
+DigitalIn *dir = NULL;
 
 
 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
 CANMessage   rxMsg;
 CANMessage   txMsg;
 
-
 SPI drv_spi(PA_7, PA_6, PA_5);
 DigitalOut drv_cs(PA_4);
-//DigitalOut drv_en_gate(PA_11);
 DRV832x drv(&drv_spi, &drv_cs);
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);  
@@ -58,9 +68,87 @@
 volatile int state = REST_MODE;
 volatile int state_change;
 
+
+void serial_interrupt(void);
+void stepInt();
+void cond_printf(const char *format, ...);
+
+// set the current mode of the UART connector
+void set_io_mode(int new_io_mode) {    
+    if (new_io_mode == IO_MODE_SERIAL) {
+        cond_printf("UART Mode\n\r");
+        io_mode = IO_MODE_SERIAL;
+        if (step != NULL)
+            step->rise(NULL);
+        delete step;
+        delete dir;
+        pc = new Serial(UART_TX, UART_RX);
+        pc->baud(UART_BAUD);
+        pc->attach(&serial_interrupt);
+        wait(100);
+        
+    }
+    else if (new_io_mode == IO_MODE_STEP_DIR){
+        cond_printf("STEP/DIR Mode\n\r");
+        wait(1000);
+        
+        io_mode = IO_MODE_STEP_DIR;        
+        
+        if (pc != NULL) {
+            pc->attach(NULL);
+        }
+        delete pc;
+
+        step = new InterruptIn(UART_RX);
+        step->rise(&stepInt);
+        dir = new DigitalIn(UART_TX);
+    }
+    
+    
+}
+
+// Checks to see if in Serial mode before printing
+void cond_printf(const char *format, ...)
+{    
+    if (io_mode != IO_MODE_SERIAL) {
+        return;
+    }
+    
+    char loc_buf[64];
+    char * temp = loc_buf;
+    va_list arg;
+    va_list copy;
+    va_start(arg, format);
+    va_copy(copy, arg);
+    size_t len = vsnprintf(NULL, 0, format, arg);
+    va_end(copy);
+    if(len >= sizeof(loc_buf)){
+        temp = new char[len+1];
+        if(temp == NULL) {
+            return;
+        }
+    }
+    len = vsnprintf(temp, len+1, format, arg);
+    pc->printf( temp);
+    va_end(arg);
+    if(len > 64){
+        delete[] temp;
+    }    
+}
+
+// Interupt function for receiving step signal
+void stepInt() {
+    if (dir) {
+        controller.p_des += RADS_PER_STEP;
+    } else {
+        controller.p_des -= RADS_PER_STEP;    
+    }
+}
+
+// CAN message received
 void onMsgReceived() {
     //msgAvailable = true;
-    //printf("%df\n\r", rxMsg.id);
+    //cond_printf("%df\n\r", rxMsg.id);
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
         controller.timeout = 0;
@@ -76,61 +164,82 @@
         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();
             }
+        // new commands ....    
+        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]==0xFB))){
+            io_mode = IO_MODE_STEP_DIR;                    
+            //if (pc != NULL) {
+                pc->attach(NULL);
+            //}            
+            step = new InterruptIn(UART_RX);
+            step->rise(&stepInt);
+            dir = new DigitalIn(UART_TX);
+        }
+        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]==0xFA))){
+            io_mode = IO_MODE_SERIAL;
+            if (step != NULL)
+                step->rise(NULL);
+            delete step;
+            delete dir;
+            pc = new Serial(UART_TX, UART_RX);
+            pc->baud(UART_BAUD);
+            pc->attach(&serial_interrupt);
+            wait(100);
+        }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
+            
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
-        }
-    
+        }    
 }
 
 void enter_menu_state(void){
     drv.disable_gd();
     //gpio.enable->write(0);
-    printf("\n\r\n\r\n\r");
-    printf(" Commands:\n\r");
+    cond_printf("\n\r\n\r\n\r");
+    cond_printf(" Commands:\n\r");
     wait_us(10);
-    printf(" m - Motor Mode\n\r");
+    cond_printf(" m - Motor Mode\n\r");
     wait_us(10);
-    printf(" c - Calibrate Encoder\n\r");
+    cond_printf(" c - Calibrate Encoder\n\r");
     wait_us(10);
-    printf(" s - Setup\n\r");
+    cond_printf(" s - Setup\n\r");
     wait_us(10);
-    printf(" e - Display Encoder\n\r");
+    cond_printf(" e - Display Encoder\n\r");
     wait_us(10);
-    printf(" z - Set Zero Position\n\r");
+    cond_printf(" z - Set Zero Position\n\r");
     wait_us(10);
-    printf(" f - Move Forward\n\r");
+    cond_printf(" f - Move Forward\n\r");
     wait_us(10);
-    printf(" b - Move Back\n\r");
+    cond_printf(" b - Move Back\n\r");
     wait_us(10);
-    printf(" p - current posiiton\n\r");
+    cond_printf(" p - current posiiton\n\r");
     wait_us(10);
-    printf(" esc - Exit to Menu\n\r");
+    cond_printf(" esc - Exit to Menu\n\r");
     wait_us(10);
     state_change = 0;
     gpio.led->write(0);
     }
 
 void enter_setup_state(void){
-    printf("\n\r\n\r Configuration Options \n\r\n\n");
+    cond_printf("\n\r\n\r Configuration Options \n\r\n\n");
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
+    cond_printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+    cond_printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+    cond_printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
+    cond_printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX);
+    cond_printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX);
+    cond_printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
+    cond_printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
     wait_us(10);
-    printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
+    cond_printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
     wait_us(10);
     state_change = 0;
     }
@@ -145,7 +254,7 @@
     controller.i_q_ref = 0;                                                     // Current Setpoints
     gpio.led->write(1);                                                     // Turn on status LED
     state_change = 0;
-    printf("\n\r Entering Motor Mode \n\r");
+    cond_printf("\n\r Entering Motor Mode \n\r");
     }
     
 void calibrate(void){
@@ -156,15 +265,15 @@
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
     gpio.led->write(0);;                                                     // Turn off status LED
     wait(.2);
-    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
+    cond_printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     drv.disable_gd();
     //gpio.enable->write(0);
      state_change = 0;
     }
     
 void print_encoder(void){
-    printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
-    //printf("%d\n\r", spi.GetRawPosition());
+    cond_printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+    //cond_printf("%d\n\r", spi.GetRawPosition());
     wait(.001);
     }
 
@@ -216,7 +325,7 @@
                     controller.ovp_flag = 1;
                     state = REST_MODE;
                     state_change = 1;
-                    printf("OVP Triggered!\n\r");
+                    cond_printf("OVP Triggered!\n\r");
                     }
                     */  
 
@@ -256,9 +365,10 @@
 
 /// 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();
+    while(pc->readable()){
+        char c = pc->getc();
         if(c == 27){
                 state = REST_MODE;
                 state_change = 1;
@@ -295,7 +405,7 @@
                         prefs.close();    
                         prefs.load(); 
                     spi.SetMechOffset(M_OFFSET);
-                    printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
+                    cond_printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                     
                     break;
                 }
@@ -323,7 +433,7 @@
                         CAN_TIMEOUT = atoi(cmd_val);
                         break;
                     default:
-                        printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
+                        cond_printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
                         break;
                     }
                     
@@ -342,7 +452,7 @@
                     cmd_val[char_count-1] = c;
                     
                 }
-                pc.putc(c);
+                pc->putc(c);
                 char_count++;
                 }
             }
@@ -362,14 +472,14 @@
                     break;
                 case 'f': // move forward                    
                     controller.p_des += 0.02f;
-                    printf("p_des: %.3f\r\n", controller.p_des);
+                    cond_printf("p_des: %.3f\r\n", controller.p_des);
                     break;
                 case 'r': // move back
                     controller.p_des -= 0.02f;
-                    printf("p_des: %.3f\r\n", controller.p_des);
+                    cond_printf("p_des: %.3f\r\n", controller.p_des);
                     break;
                 case 'p': // show posiiton                                                               
-                    printf("Pos: %.3f Vel: %.3f Cur: %.3f\r\n", controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+                    cond_printf("Pos: %.3f Vel: %.3f Cur: %.3f\r\n", controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
                     break;    
                 }
                 
@@ -377,6 +487,7 @@
             
         }
     }
+
        
 int main() {
     controller.v_bus = V_BUS;
@@ -439,48 +550,27 @@
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     init_controller_params(&controller);
 
-    pc.baud(921600);                                                            // set serial baud rate
+    
+    set_io_mode(IO_MODE_SERIAL);
     wait_us(100);
-    pc.printf("\n\r\n\r Hobby King Cheetah\n\r\n\r");
+    cond_printf("\n\r\n\r Hobby King Cheetah\n\r\n\r");
     wait_us(10);
-    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(" Output Zero Position:  %.4f\n\r", M_OFFSET);
-    printf(" Gear Ratio %.4f:1\r\n", GR);
-    printf(" Mapped Position %.4f to %.4f Radians\n\r", P_MIN, P_MAX);
-    printf(" CAN ID:  %d\n\r", CAN_ID);
-    
-
-
-
-    //printf(" %d\n\r", drv.read_register(DCR));
-    //wait_us(100);
-    //printf(" %d\n\r", drv.read_register(CSACR));
-    //wait_us(100);
-    //printf(" %d\n\r", drv.read_register(OCPCR));
-    //drv.disable_gd();
-    
-    pc.attach(&serial_interrupt);                                               // attach serial interrupt
-    
+    cond_printf("\n\r Debug Info:\n\r");
+    cond_printf(" Firmware Version: %s\n\r", VERSION_NUM);
+    cond_printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
+    cond_printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
+    cond_printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
+    cond_printf(" Gear Ratio %.4f:1\r\n", GR);
+    cond_printf(" Mapped Position %.4f to %.4f Radians\n\r", P_MIN, P_MAX);
+    cond_printf(" CAN ID:  %d\n\r", CAN_ID);
+        
     state_change = 1;
 
-
-    int counter = 0;
+    //int counter = 0;
     while(1) {
         drv.print_faults();
        wait(.1);
-       //printf("%.4f\n\r", controller.v_bus);
-       /*
-        if(state == MOTOR_MODE)
-        {
-            //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
-            //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
-            //printf("%.3f\n\r", controller.dtheta_mech);
-            wait(.002);
-        }
-        */
+       
 
     }
 }