Kenji Arai
DRV8830/TI Motor Driver sample program.\\ This program can control two motors.
Diff: main.cpp
- Revision:
- 4:58734155cd29
- Parent:
- 3:db817fb05ba7
--- 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