Kenji Arai
/
NucleoF401_motor_test_simple
DRV8830/TI Motor Driver sample program.\\ This program can control two motors.
Diff: main.cpp
- Revision:
- 4:58734155cd29
- Parent:
- 3:db817fb05ba7
diff -r db817fb05ba7 -r 58734155cd29 main.cpp --- a/main.cpp Sat Jul 12 11:55:37 2014 +0000 +++ b/main.cpp Sun Aug 24 00:52:11 2014 +0000 @@ -1,11 +1,14 @@ /* - * mbed Application program for the mbed ST NUCLEO F401RE Board + * mbed Application program for the mbed + * Brushed DC Motor control + * Texas Instruments / DRV8830 H-Bridge Voltage-Controlled Motor Driver + * http://www.ti.com/product/drv8830 * * Copyright (c) 2014 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ * Created: July 12th, 2014 - * Revised: July 12th, 2014 + * Revised: August 24th, 2014 * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE @@ -16,37 +19,305 @@ // Include --------------------------------------------------------------------------------------- #include "mbed.h" -#include "rtos.h" +#include "DRV8830.h" // Object ---------------------------------------------------------------------------------------- -DigitalOut myled(LED1); -DigitalIn usr_sw(PC_13); +DigitalOut myled(LED1); +DigitalIn usr_sw(PC_13); +Serial pc(USBTX, USBRX); // Communication with Host +I2C i2c(PB_9,PB_8); // SDA, SCL +DRV8830 right_mtr(i2c, (uint8_t)DRV8830ADDR_00); +DRV8830 left_mtr(i2c, (uint8_t)DRV8830ADDR_01); + +// Definition ------------------------------------------------------------------------------------ +#define BAUD(x) pc.baud(x) +#define GETC(x) pc.getc(x) +#define PUTC(x) pc.putc(x) +#define PRINTF(...) pc.printf(__VA_ARGS__) +#define READABLE(x) pc.readable(x) + +#define DO_DEBUG +#ifdef DO_DEBUG +#define DEBUG(...) printf(__VA_ARGS__) +#else +#define DEBUG(...) {;} +#endif + +// RAM ------------------------------------------------------------------------------------------- +char lb[32]; + +// ROM / Constant data --------------------------------------------------------------------------- +static char *const msg0 = "Brushed DC Motor Control by JH1PJL, created on "__DATE__""; // Function prototypes --------------------------------------------------------------------------- -extern int mon( void); //------------------------------------------------------------------------------------------------- // Control Program //------------------------------------------------------------------------------------------------- -int read_sw (void){ - if (usr_sw == 0){ return 1; - } else { return 0;} +// Communication with Host PC --------------------------------------------------------------------- +// Put \r\n +void put_rn ( void ){ PUTC('\r'); PUTC('\n');} + +// Put \r +void put_r ( void ){ PUTC('\r');} + +// Put ", " +void put_lin ( void ){ PRINTF(", ");} + +// Put space n +void put_spc( uint8_t n){ for(;n > 0; n--){ PUTC(' '); }} + +// Change string -> number +int xatoi (char **str, int32_t *res){ +unsigned long val; +unsigned char c, radix, s = 0; + + while ((c = **str) == ' '){ (*str)++;} + if (c == '-') { + s = 1; + c = *(++(*str)); + } else if (c == '+') { + s = 0; + c = *(++(*str)); + } + if (c == '0') { + c = *(++(*str)); + if (c <= ' ') { *res = 0; return 1; } + if (c == 'x') { + radix = 16; + c = *(++(*str)); + } else { + if (c == 'b') { + radix = 2; + c = *(++(*str)); + } else { + if ((c >= '0')&&(c <= '9')){ + radix = 8; + } else { + return 0; + } + } + } + } else { + if ((c < '1')||(c > '9')){ return 0;} + radix = 10; + } + val = 0; + while (c > ' ') { + if (c >= 'a'){ c -= 0x20;} + c -= '0'; + if (c >= 17){ + c -= 7; + if (c <= 9){ return 0;} + } + if (c >= radix){ return 0;} + val = val * radix + c; + c = *(++(*str)); + } + if (s){ val = -val;} + *res = val; + return 1; } -// Monitor program -void mon_thread(void const *args){ - while (true){ mon(); } +// Get key input data +void get_line (char *buff, int len){ +char c; +int idx = 0; + + for (;;) { + c = GETC(); + if (c == '\r') { + buff[idx++] = c; + break; + } + if ((c == '\b') && idx) { + idx--; + PUTC(c); + PUTC(' '); + PUTC(c); + } + if (((uint8_t)c >= ' ') && (idx < len - 1)) { + buff[idx++] = c; + PUTC(c); + } + } + buff[idx] = 0; + PUTC('\n'); +} + +void help(void){ + PRINTF("0 -> select Right motor"); + put_rn(); + PRINTF("1 -> select Left motor"); + put_rn(); + PRINTF("2 -> select Both motors"); + put_rn(); + PRINTF("m +/-<speed> x100, e.g. -0.35 -> enter -35"); + put_rn(); + PRINTF("c -> Change spped automatically"); + put_rn(); + PRINTF("s -> Stop"); + put_rn(); + PRINTF("e -> Check error status"); + put_rn(); + PRINTF("r -> Reset error"); + put_rn(); +} + +// Motor control ---------------------------------------------------------------------------------- +void ctrl_speed(float spd, uint8_t mtr_selct){ + switch (mtr_selct){ + case 0 : + right_mtr.speed(spd); + break; + case 1 : + left_mtr.speed(spd); + break; + case 2 : + right_mtr.speed(spd); + wait(0.001); + left_mtr.speed(spd); + break; + default: + ; + } } -// Thread definition -osThreadDef(mon_thread, osPriorityNormal, 2048); +void show_status(uint8_t status){ + PRINTF("Status:0x%02x ->", status); + if (status & DRV8830_F_FAULT){ + PRINTF("Faulted "); + if (status & DRV8830_F_ILIMIT){ + PRINTF("/Current limit "); + } + if (status & DRV8830_F_OTS){ + PRINTF("/Over temp. "); + } + if (status & DRV8830_F_UVLO){ + PRINTF("/Under V lockout "); + } + if (status & DRV8830_F_OCP){ + PRINTF("/Over current "); + } + } else { + PRINTF(" No fault"); + } + put_rn(); +} +// Main control ----------------------------------------------------------------------------------- int main() { - // Starts 1st thread - osThreadCreate(osThread(mon_thread), NULL); - while(1) { - myled = !myled; - Thread::wait(1000); +int32_t p1; +char *ptr; +float spd = 0.0; +uint8_t status; +uint8_t stop_flag = 0; +uint8_t mtr_selct = 0; + + put_rn(); + PRINTF(msg0); + put_rn(); + for (;;) { + put_r(); + PUTC('>'); + myled = 0; + ptr = lb; + get_line(ptr, sizeof(lb)); + myled = 1; + switch (*ptr++){ + case '0' : + put_r(); + mtr_selct = 0; + PRINTF("Select Right Motor"); + put_rn(); + break; + case '1' : + put_r(); + mtr_selct = 1; + PRINTF("Select Left Motor"); + put_rn(); + break; + case '2' : + put_r(); + mtr_selct = 2; + PRINTF("Select Both Motors"); + put_rn(); + break; + case 'e' : + put_r(); + PRINTF("Right "); + status = right_mtr.status(); + show_status(status); + PRINTF("Left "); + status = left_mtr.status(); + show_status(status); + break; + case 'm' : + if (xatoi(&ptr, &p1)) { + spd = (float)p1 / 100; + } + ctrl_speed(spd, mtr_selct); + put_r(); + PRINTF("Run "); + put_rn(); + break; + case 'r' : + put_r(); + left_mtr.reset(); + PRINTF("Reset error"); + put_rn(); + right_mtr.reset(); + break; + case 'c' : + put_r(); + PRINTF("CW ++speed"); + put_rn(); + for (spd = 0.0; spd <= 1.0; spd += 0.02){ + ctrl_speed(spd, mtr_selct); + if (READABLE()){ GETC(); stop_flag =1; break;} + wait(0.1); + } + if (stop_flag){ stop_flag =0; break;} + PRINTF("CW --speed"); + put_rn(); + for (spd = 1.0; spd >= 0.1; spd -= 0.02){ + ctrl_speed(spd, mtr_selct); + if (READABLE()){ GETC(); stop_flag =1; break;} + wait(0.1); + } + if (stop_flag){ stop_flag =0; break;} + PRINTF("CCW ++speed"); + put_rn(); + for (spd = 0.0; spd >= -1.0; spd -= 0.02){ + ctrl_speed(spd, mtr_selct); + if (READABLE()){ GETC(); stop_flag =1; break;} + wait(0.1); + } + if (stop_flag){ stop_flag =0; break;} + PRINTF("CCW --speed"); + put_rn(); + for (spd = -1.0; spd <= 0.0; spd += 0.02){ + ctrl_speed(spd, mtr_selct); + if (READABLE()){ GETC(); stop_flag =1; break;} + wait(0.1); + } + if (stop_flag){ stop_flag =0; break;} + // break; + case 's' : + ctrl_speed(0.0, mtr_selct); + put_r(); + PRINTF("Stop"); + put_rn(); + break; + case '?' : + put_r(); + help(); + break; + default: + put_r(); + PRINTF("Help ->?"); + put_rn(); + help(); + } } } - \ No newline at end of file