Thomas Byrne
testing
Dependencies: mbed tinyshell X_NUCLEO_IHM02A1
Diff: main.cpp
- Revision:
- 28:19b25daa7777
- Parent:
- 27:2abcf13d90a3
- Child:
- 29:a510e875936d
--- a/main.cpp Wed Mar 31 22:28:53 2021 +0000
+++ b/main.cpp Thu Apr 01 00:43:00 2021 +0000
@@ -48,6 +48,8 @@
/* Expansion Board specific header files. */
#include "XNucleoIHM02A1.h"
+/* Tinyshell: https://os.mbed.com/users/murilopontes/code/tinyshell/ */
+#include "tinysh.h"
/* Definitions ---------------------------------------------------------------*/
@@ -67,14 +69,22 @@
#define OCD_TH_MA 600.0
#define STALL_TH_MA 1000.0
-/* Number of movements per revolution. */
-#define MPR_1 4
+/* Behavioral stuff */
+#define STATUS_LOG_RATE_HZ 1
+#define MIN_LOOP_TIME_MS 1
+#define HARD_STOP_WAIT_MS 100
+
+#define CMD_BUFFER_SIZE 256
+#define OUTPUT_BUFFER_SIZE 1024
/* Number of steps. */
#define STEPS_1 (20000 * 1) /* 1 revolution given a 200 step motor and 100:1 gearbox, with full stepping */
#define STEPS_2 (STEPS_1 * 2)
#define SPEED_SPS 10000 // steps per second
+/* Number of movements per revolution. */
+#define MPR_1 4
+
/* Delay in milliseconds. */
#define DELAY_1 1000
#define DELAY_2 2000
@@ -145,12 +155,45 @@
}
};
+/* Serial Port for console */
+Serial pc(USBTX, USBRX);
+
+/* Nasty globals haha */
+bool limit_flag = false;
+bool status_flag = false;
+char cmd_buffer[CMD_BUFFER_SIZE];
+char output_buffer[OUTPUT_BUFFER_SIZE];
+
+/* Tinyshell command handler functions */
+void print_status(int argc, char **argv)
+{
+ status_flag = true;
+}
+
+// parent cmd (0 for top) cmd input name, help string, usage string,
+// function to launch, arg when called, next (0 at init), child (0 at init)
+tinysh_cmd_t print_status_cmd = {0, "status", "status command", "[args]", print_status,0,0,0};
+
+/* mandatory tiny shell output function */
+void tinysh_char_out(unsigned char c)
+{
+ pc.putc(c);
+}
/* Main ----------------------------------------------------------------------*/
int main()
{
/*----- Initialization. -----*/
+ unsigned int status_bytes[L6470DAISYCHAINSIZE];
+
+ /* Set up tinyshell */
+ pc.baud(115200);
+ pc.printf("Motor controller ready\r\n");
+ tinysh_set_prompt("$ ");
+
+ // Add all tinyshell commands here
+ tinysh_add_command(&print_status_cmd);
/* Initializing SPI bus. */
#ifdef TARGET_STM32F429
@@ -165,19 +208,83 @@
/* Building a list of motor control components. */
L6470 **motors = x_nucleo_ihm02a1->get_components();
- /* Printing to the console. */
- printf("Motor Control Application Example for 2 Motors\r\n\n");
-
-
- /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
+ // Hello world: just run the damn motor forever
+ printf("Running.\r\n");
for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
motors[m]->prepare_run(StepperMotor::BWD, SPEED_SPS);
}
-
- /* Performing the action on each motor at the same time. */
x_nucleo_ihm02a1->perform_prepared_actions();
- printf("Running.\r\n");
+
+ // Main loop
+ while(1) {
+ /* 1: Check for hardware flags ----------------------------------------*/
+ if (limit_flag) {
+ // Hard stop
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_hard_stop();
+ }
+ x_nucleo_ihm02a1->perform_prepared_actions();
+
+ wait_ms(HARD_STOP_WAIT_MS);
+
+ // High Z
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ motors[m]->prepare_hard_hiz();
+ }
+ x_nucleo_ihm02a1->perform_prepared_actions();
+
+ printf("Reached limit.\r\n");
+ limit_flag = false;
+ }
+
+ /* 2: Fetch new chars for Tinyshell ----------------------------------*/
+ tinysh_char_in(pc.getc());
+
+ /* 3: Handle Commands ------------------------------------------------*/
+ if (status_flag) {
+ // Fetch and parse the status register for each motor
+ for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ status_bytes[m] = motors[m]->get_status();
+ printf("Status reg %d: 0x%02X\r\n", m, status_bytes[m]);
+ printf(" STEP-CLOCK MODE: ");
+ printf(status_bytes[m] & 0x8000 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" STEP_LOSS_B: ");
+ printf(status_bytes[m] & 0x4000 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" STEP_LOSS_A: ");
+ printf(status_bytes[m] & 0x2000 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" OVERCURRENT DETECT: ");
+ printf(status_bytes[m] & 0x1000 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" THERMAL SHUTDOWN: ");
+ printf(status_bytes[m] & 0x0800 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" THERMAL WARN: ");
+ printf(status_bytes[m] & 0x0400 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" UNDERVOLTAGE LOCKOUT: ");
+ printf(status_bytes[m] & 0x0200 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" WRONG_CMD: ");
+ printf(status_bytes[m] & 0x0100 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" NOTPERF_CMD: ");
+ printf(status_bytes[m] & 0x0080 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" MOTOR_STATUS: ");
+ if ((status_bytes[m] && 0x0060) >> 5 == 0x00) printf("STOPPED\r\n");
+ if ((status_bytes[m] && 0x0060) >> 5 == 0x01) printf("ACCELERATING\r\n");
+ if ((status_bytes[m] && 0x0060) >> 5 == 0x10) printf("DECELERATING\r\n");
+ if ((status_bytes[m] && 0x0060) >> 5 == 0x11) printf("CONSTANT SPEED\r\n");
+ printf(" DIRECTION: ");
+ printf(status_bytes[m] & 0x0010 ? "FWD\r\n" : "REV\r\n");
+ printf(" SWITCH TURN-ON EVENT: ");
+ printf(status_bytes[m] & 0x0008 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" SWITCH STATUS: ");
+ printf(status_bytes[m] & 0x0004 ? "CLOSED\r\n" : "OPEN\r\n");
+ printf(" BUSY: ");
+ printf(status_bytes[m] & 0x0002 ? "SET\r\n" : "NOT SET\r\n");
+ printf(" HI_Z: ");
+ printf(status_bytes[m] & 0x0001 ? "SET\r\n" : "NOT SET\r\n");
+ printf("\n\n");
+ }
+ }
+
+ } // end main loop
}
@@ -400,23 +507,6 @@
// wait_ms(DELAY_1);
//
//
-// /*----- Hard Stop. -----*/
-//
-// /* Printing to the console. */
-// printf("--> Hard Stop.\r\n");
-//
-// /* Preparing each motor to perform a hard stop. */
-// for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
-// motors[m]->prepare_hard_stop();
-// }
-//
-// /* Performing the action on each motor at the same time. */
-// x_nucleo_ihm02a1->perform_prepared_actions();
-//
-// /* Waiting. */
-// wait_ms(DELAY_2);
-//
-//
// /*----- Doing a full revolution on each motor, one after the other. -----*/
//
// /* Printing to the console. */
@@ -442,20 +532,4 @@
// /* Waiting. */
// wait_ms(DELAY_2);
//
-//
-// /*----- High Impedance State. -----*/
-//
-// /* Printing to the console. */
-// printf("--> High Impedance State.\r\n");
-//
-// /* Preparing each motor to set High Impedance State. */
-// for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
-// motors[m]->prepare_hard_hiz();
-// }
-//
-// /* Performing the action on each motor at the same time. */
-// x_nucleo_ihm02a1->perform_prepared_actions();
-//
-// /* Waiting. */
-// wait_ms(DELAY_2);
//}