nhan pham an
Fork and fix for mwork
Dependencies: mbed-dev-f303 FastPWM3 millis
Diff: main.cpp
- Revision:
- 54:3e056b097c52
- Parent:
- 53:349304b6d937
- Child:
- 55:fee62d8fd8fb
--- 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);
- }
- */
+
}
}