Diff: main.cpp

Revision:

0:8cd487e115a3

Child:

1:8af23374bd90

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 04 20:17:02 2016 +0000
@@ -0,0 +1,442 @@
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Helper header files. */
+#include "DevSPI.h"
+
+/* Component specific header files. */
+#include "l6474_class.h"
+
+
+
+#include "tinysh.h"
+ 
+/* Definitions ---------------------------------------------------------------*/
+
+/* Number of steps. */
+#define STEPS_1 (800 * 8)   /* 1 revolution given a 400 steps motor configured at 1/8 microstep mode. */
+
+/* Delay in milliseconds. */
+#define DELAY_1 1000
+#define DELAY_2 2000
+#define DELAY_3 6000
+#define DELAY_4 8000
+
+/* Speed in pps (Pulses Per Second).
+   In Full Step mode: 1 pps = 1 step/s).
+   In 1/N Step Mode:  N pps = 1 step/s). */
+#define SPEED_1 2400
+#define SPEED_2 1200
+
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Initialization parameters. */
+L6474_Init_t init =
+{
+    500,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
+    500,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
+    300,                             /* Maximum speed in pps. Range: (30..10000]. */
+    300,                              /* Minimum speed in pps. Range: [30..10000). */
+    1250,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
+    L6474_OCD_TH_2250mA,               /* Overcurrent threshold (OCD_TH register). */
+    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
+    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
+    L6474_STEP_SEL_1_16,               /* Step selection (STEP_SEL field of STEP_MODE register). */
+    L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
+    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
+    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
+    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
+    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
+    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
+    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
+    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
+    L6474_ALARM_EN_OVERCURRENT |
+    L6474_ALARM_EN_THERMAL_SHUTDOWN |
+    L6474_ALARM_EN_THERMAL_WARNING |
+    L6474_ALARM_EN_UNDERVOLTAGE |
+    L6474_ALARM_EN_SW_TURN_ON |
+    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
+};
+
+/* Motor Control Component. */
+L6474 *motor;
+
+// User Button
+DigitalIn userBtn(USER_BUTTON);
+DigitalIn a1Btn(A1);
+DigitalIn a2Btn(A2);
+DigitalIn a3Btn(A3);
+DigitalOut buzzer(D3);
+
+// serial port to use 
+Serial pc(USBTX, USBRX);
+ 
+
+/* Functions -----------------------------------------------------------------*/
+
+void PrintAssertedStatusFlags(unsigned int status) {
+    // The following flags are ACTIVE HIGH
+    
+    /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
+    /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
+    if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD)
+        printf("    WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
+    
+    if ((status & L6474_STATUS_WRONG_CMD) == L6474_STATUS_WRONG_CMD)
+        printf("    WARNING: \"FLAG\" interrupt triggered. L6474_STATUS_WRONG_CMD\r\n");
+    
+    
+    // The following flags are ACTIVE LOW
+    
+    if ((status & L6474_STATUS_UVLO) == 0)
+        printf("    WARNING: \"FLAG\" interrupt triggered. L6474_STATUS_UVLO\r\n");
+    
+    if ((status & L6474_STATUS_TH_WRN) == 0)
+        printf("    WARNING: \"FLAG\" interrupt triggered. L6474_STATUS_TH_WRN\r\n");
+    
+    if ((status & L6474_STATUS_TH_SD) == 0)
+        printf("    WARNING: \"FLAG\" interrupt triggered. L6474_STATUS_TH_SD\r\n");
+    
+    if ((status & L6474_STATUS_OCD) == 0)
+        printf("    WARNING: \"FLAG\" interrupt triggered. L6474_STATUS_OCD\r\n");
+}
+
+/**
+ * @brief  This is an example of user handler for the flag interrupt.
+ * @param  None
+ * @retval None
+ * @note   If needed, implement it, and then attach and enable it:
+ *           + motor->AttachFlagIRQ(&FlagIRQHandler);
+ *           + motor->EnableFlagIRQ();
+ *         To disable it:
+ *           + motor->DisbleFlagIRQ();
+ */
+void FlagIRQHandler(void)
+{
+    /* Set ISR flag. */
+    motor->isr_flag = TRUE;
+
+    /* Get the value of the status register. */
+    unsigned int status = motor->GetStatus();
+    
+    printf("    WARNING: FLAG interrupt, status=%d\r\n", status);
+    
+    PrintAssertedStatusFlags(status);
+    
+    /* Reset ISR flag. */
+    motor->isr_flag = FALSE;
+}
+
+void ErrorHandler(uint16_t errorCode) {
+    printf(" ERROR CONDITION %d, HALTING DEVICE\r\n", errorCode);
+    while(1); //Endless loop
+}
+
+void PrintStatus() {
+    printf("Position: %d    ", motor->GetPosition());
+    printf("Speed: %d  (%d - %d)    ", motor->GetSpeed(), motor->GetMinSpeed(), motor->GetMaxSpeed());
+    printf("Mark: %d    ", motor->GetMark());
+    printf("Driver Status: 0x%X    ", motor->ReadStatusRegister());
+    printf("Device State: 0x%X    ", motor->GetDeviceState());
+    PrintAssertedStatusFlags(motor->ReadStatusRegister());
+    printf("\r\n");
+}
+
+/* Shell Commands ----------------------------------------------------------------------*/
+
+
+#define SHELL_COMMAND(name, description, arg_help) \
+    void on_ ## name (int argc, char **argv); \
+    tinysh_cmd_t cmd_ ## name = {0, #name, description, arg_help, on_ ## name, 0, 0, 0}; \
+    void on_ ## name (int argc, char **argv)
+
+#define SHELL_SUBCOMMAND(parent, name, description, arg_help) \
+    void on_ ## name (int argc, char **argv); \
+    tinysh_cmd_t cmd_ ## parent ## _ ## name = {&cmd_ ## parent, #name, description, arg_help, on_ ## name, 0, 0, 0}; \
+    void on_ ## name (int argc, char **argv)
+
+#define SHELL_COMMAND_NOFUN(name, description, arg_help) \
+    tinysh_cmd_t cmd_ ## ## name = {0, #name, description, arg_help, 0, 0, 0, 0};
+
+#define SHELL_SUBCOMMAND_NOFUN(parent, name, description, arg_help) \
+    tinysh_cmd_t cmd_ ## parent ## _ ## name = {&cmd_ ## parent, #name, description, arg_help, 0, 0, 0, 0};
+
+
+SHELL_COMMAND(mf, "move forward", "[steps=1]")
+{
+    uint32_t steps = atoi(argv[1]);
+    if (!steps) steps = 1;
+    motor->Move(StepperMotor::FWD, steps);
+}
+
+SHELL_COMMAND(mb, "move backward", "[steps=1]")
+{
+    uint32_t steps = atoi(argv[1]);
+    if (!steps) steps = 1;
+    motor->Move(StepperMotor::BWD, steps);
+}
+
+SHELL_COMMAND(rf, "run forward", "")
+{
+    motor->Run(StepperMotor::FWD);
+}
+SHELL_COMMAND(rb, "run backward", "")
+{
+    motor->Run(StepperMotor::BWD);
+}
+
+SHELL_COMMAND_NOFUN(set, "set preferences", "")
+
+SHELL_SUBCOMMAND(set, maxspeed, "set maximum speed", "speed in pps")
+{
+    unsigned int value = atoi(argv[1]);
+    if (!value) printf("Error: missing argument 'speed in pps'\r\n");
+    else motor->SetMaxSpeed(value);
+}
+SHELL_SUBCOMMAND(set, minspeed, "set minimum speed", "speed in pps")
+{
+    unsigned int value = atoi(argv[1]);
+    if (!value) printf("Error: missing argument 'speed in pps'\r\n");
+    else motor->SetMinSpeed(value);
+}
+SHELL_SUBCOMMAND(set, speed, "set speed", "speed in pps")
+{
+    unsigned int value = atoi(argv[1]);
+    if (!value) printf("Error: missing argument 'speed in pps'\r\n");
+    else  {
+        motor->SetMaxSpeed(value);
+        motor->SetMinSpeed(value);
+        motor->SetMaxSpeed(value);
+        }
+    
+    if (argc > 2 && (value = atoi(argv[2]))) {
+        if (!value) printf("Error: invalid argument 'acc/dec in pps^2'\r\n");
+        else  {
+            motor->SetAcceleration(value);
+            motor->SetDeceleration(value);
+        }
+    }
+}
+SHELL_SUBCOMMAND(set, acceleration, "set acceleration", "acc in pps^2")
+{
+    unsigned int value = atoi(argv[1]);
+    if (!value) printf("Error: missing argument 'acc in pps^2'\r\n");
+    else motor->SetAcceleration(value);
+}
+SHELL_SUBCOMMAND(set, deceleration, "set deceleration", "dec in pps^2")
+{
+    unsigned int value = atoi(argv[1]);
+    if (!value) printf("Error: missing argument 'dec in pps^2'\r\n");
+    else motor->SetDeceleration(value);
+}
+SHELL_SUBCOMMAND(set, stepmode, "set step mode", "0=full, 1=half, 2=1/4, 3=1/8, 4=1/16")
+{
+    unsigned int value = atoi(argv[1]);
+    if (value>4) printf("Error: invalid argument 'stepmode'\r\n");
+    motor->SetStepMode((StepperMotor::step_mode_t)value);
+}
+
+SHELL_COMMAND(stop, "stop motor", "[hard]")
+{
+    if (argv[1][0] == 'h') motor->HardStop();
+    else motor->SoftStop();
+}
+
+SHELL_COMMAND(kill, "stop motor immediately and set in HiZ mode", "")
+{
+    motor->HardHiZ();
+}
+
+SHELL_COMMAND(hiz, "stop motor and set in HiZ mode", "[hard]")
+{
+    if (argv[1][0] == 'h') motor->HardHiZ();
+    else motor->SoftHiZ();
+}
+
+SHELL_COMMAND(sethome, "set home here", "")
+{
+    motor->SetHome();
+}
+
+SHELL_COMMAND(go, "go to position (default is home)", "[position=0]")
+{
+    uint32_t position = atoi(argv[1]);
+    if (!position) position = 1;
+    motor->GoTo(position);
+}
+
+SHELL_COMMAND(setmark, "set mark here", "")
+{
+    motor->SetMark();
+}
+
+SHELL_COMMAND(gomark, "go to mark", "")
+{
+    motor->GoMark();
+}
+
+SHELL_COMMAND(status, "print status information", "")
+{
+    PrintStatus();
+}
+SHELL_COMMAND(wait, "wait for motor", "")
+{
+    motor->WaitWhileActive();
+}
+
+SHELL_COMMAND_NOFUN(debug, "debugging tools", "")
+
+SHELL_SUBCOMMAND(debug, test1, "print the command arguments", "[args ...]")
+{
+    printf("test1 command called\r\n");
+    for(int i=0; i<argc; i++) {
+        printf("argv[%d]=\"%s\"\r\n",i,argv[i]);
+    }
+}
+SHELL_SUBCOMMAND(debug, regs, "print all or the specified L6474 registers", "[index ...]")
+{
+    int i;
+    if (argc>1) {
+        for(int k=1;k<argc; k++){
+            i = strtol(argv[k], NULL, 0);
+            printf("reg[0x%X]=%f\r\n",i,motor->GetParameter(i));
+        }
+    } else {
+        for(i=0; i<0x1b; i++) {
+            printf("reg[0x%X]=%f\r\n",i,motor->GetParameter(i));
+        }
+    }
+}
+SHELL_SUBCOMMAND(debug, uptime, "uptime", "")
+{
+    printf("Uptime: %d\r\n", us_ticker_read());
+    
+}
+
+SHELL_SUBCOMMAND(debug, delay, "wait t microseconds", "t")
+{
+    unsigned long t = strtoul(argv[1], NULL, 0);
+    wait_us(t);
+}
+
+
+#define SHELL_REG_COMMAND(name) tinysh_add_command(&cmd_ ## name);
+void register_commands() {
+    
+    SHELL_REG_COMMAND(mf);
+    SHELL_REG_COMMAND(mb);
+    SHELL_REG_COMMAND(rf);
+    SHELL_REG_COMMAND(rb);
+    
+    SHELL_REG_COMMAND(set);
+    SHELL_REG_COMMAND(set_maxspeed);
+    SHELL_REG_COMMAND(set_minspeed);
+    SHELL_REG_COMMAND(set_speed);
+    SHELL_REG_COMMAND(set_acceleration);
+    SHELL_REG_COMMAND(set_deceleration);
+    SHELL_REG_COMMAND(set_stepmode);
+    
+    SHELL_REG_COMMAND(stop);
+    SHELL_REG_COMMAND(kill);
+    SHELL_REG_COMMAND(hiz);
+    
+    SHELL_REG_COMMAND(sethome);
+    SHELL_REG_COMMAND(go);
+    SHELL_REG_COMMAND(setmark);
+    SHELL_REG_COMMAND(gomark);
+    SHELL_REG_COMMAND(status);
+    SHELL_REG_COMMAND(wait);
+    SHELL_REG_COMMAND(debug);
+    SHELL_REG_COMMAND(debug_test1);
+    SHELL_REG_COMMAND(debug_regs);
+    SHELL_REG_COMMAND(debug_delay);
+    SHELL_REG_COMMAND(debug_uptime);
+}
+
+ 
+ 
+//mandatory tiny shell output function
+void tinysh_char_out(unsigned char c)
+{
+    pc.putc(c);
+}
+ 
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+    /*----- Initialization. -----*/
+
+   //configure serial baudrate
+    pc.baud(115200);
+ 
+    /* Initializing SPI bus. */
+    DevSPI dev_spi(D11, D12, D13);
+
+    /* Initializing Motor Control Component. */
+    motor = new L6474(D2, D8, D7, D6, D10, dev_spi);
+    if (motor->Init(&init) != COMPONENT_OK)
+        exit(EXIT_FAILURE);
+    motor->SetMaxSpeed(init.maximum_speed_pps);
+    
+    /* Attaching and enabling interrupt handlers. */
+    motor->AttachFlagIRQ(&FlagIRQHandler);
+    motor->EnableFlagIRQ();
+    motor->AttachErrorHandler(&ErrorHandler);
+
+    /* Printing to the console. */
+    printf("\r\nantennenlab 0.0.1\r\nCopyright (c) 2016  Max Weller\r\n");
+    printf("tiny shell build %s %s\r\n",__DATE__,__TIME__);
+    printf("Motor Driver FW Version: %d\r\n", motor->GetFwVersion());
+    PrintStatus();
+    printf("\r\n");
+    
+   //print build date
+ 
+    //set prompt
+    tinysh_set_prompt("!?\n");
+ 
+    //add custom commands here
+    register_commands();
+ 
+   int timerctr; char inchar;
+   //run command parser loop foverer 
+    while(true) {
+        while (pc.readable()) {
+            inchar = pc.getc();
+            if (inchar == 3) {
+                motor->HardHiZ();
+            }
+            tinysh_char_in( inchar );
+        }
+        if (!a1Btn) {
+            printf("Running forward... ");
+            motor->Run(StepperMotor::FWD);
+            while(!a1Btn){}
+            printf("Stopping\r\n");
+            motor->SoftStop();
+        }
+        if (!a2Btn) {
+            printf("Running backwards... ");
+            motor->Run(StepperMotor::BWD);
+            while(!a2Btn){}
+            printf("Stopping\r\n");
+            motor->SoftStop();
+        }
+        if (!a3Btn) {
+            printf("Hold to set home... ");
+            timerctr=0;
+            while(!a3Btn){timerctr++;if(timerctr==800000){printf("\rRELEASE to set home ");buzzer=1;}}
+            printf("%d\r\n", timerctr);
+            if (timerctr>800000){motor->SetHome();printf("\rSetting home\r\n");}
+            else {motor->GoHome();printf("\rGoing home\r\n");}
+            buzzer=0;
+        }
+    }
+}
+
+