One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
Revision 10:716484b1ddb5, committed 2016-04-05
- Comitter:
- vsutardja
- Date:
- Tue Apr 05 18:30:41 2016 +0000
- Parent:
- 9:10fcf3d0ec2d
- Child:
- 11:4348bba086a4
- Commit message:
- latest
Changed in this revision
--- a/main.cpp Fri Apr 01 23:47:07 2016 +0000 +++ b/main.cpp Tue Apr 05 18:30:41 2016 +0000 @@ -6,31 +6,29 @@ #include "QEI.h" #include "telemetry.h" +//extern "C" { extern int printfNB(const char *format, ...); } +//extern "C" { extern int putcharNB(int);} + // ========= // Telemetry // ========= -MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX -telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer -telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry - -int dec = 0; -Timer t; -telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); -telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); -telemetry::Numeric<float> tele_vel(telemetry_obj, "t_vel", "Velocity", "m/s", 0); -telemetry::Numeric<uint32_t> tele_center(telemetry_obj, "t_center", "Center", "", 0); +//MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX +//telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer +//telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry // ============= // Communication // ============= Serial pc(USBTX, USBRX); // USB connection -//Serial bt(PTC4, PTC3); // BlueSMiRF connection +MODSERIAL bt(PTC4, PTC3); // BlueSMiRF connection +//int idx = 0; char cmd; // Command char ch; char in[5]; void communication(void const *args); // Communications +int lock = 0; // ===== // Motor @@ -58,7 +56,7 @@ float Kp = 6.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor -float interval = 0.1; // Sampling interval +float interval = 0.01; // Sampling interval float ref_v = 1.0; PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller @@ -78,6 +76,7 @@ Timeout camera_read; // Camera read timeout int t_int = 15000; // Exposure time int img[128]; // Image data +uint8_t img_out[128]; void readCamera(); // Read data from camera @@ -98,102 +97,105 @@ // Functions // ================ -void tele_comm(void const *args) { - telemetry_serial.baud(115200); - telemetry_obj.transmit_header(); +// Communications +void communication(void const *args) { while (true) { - tele_time_ms = t.read_ms(); - telemetry_obj.do_io(); - Thread::wait(100); + bt.printf("\r\nPress q to return to this prompt.\r\n"); + bt.printf("Available diagnostics:\r\n"); + bt.printf(" [0] Velocity\r\n"); + bt.printf(" [1] Steering\r\n"); + bt.printf(" [2] Change Kp\r\n"); + bt.printf(" [3] Change Ki\r\n"); + bt.printf(" [4] Change Kd\r\n"); + bt.printf(" [5] Change Ks\r\n"); + bt.printf(" [6] Change reference velocity\r\n"); + bt.printf(" [7] Checkpoint telemetry\r\n"); + cmd = bt.getc(); + while (cmd != 'q') { + switch(atoi(&cmd)){ + case 0: + bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", d, curr_pulses, velocity, Kp, Ki, Kd); + break; + case 1: + bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); + break; + case 2: + bt.printf("Current: %f, New (5 digits): ", Kp); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Kp = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; + break; + case 3: + bt.printf("Current: %f, New (5 digits): ", Ki); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Ki = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; + break; + case 4: + bt.printf("Current: %f, New (5 digits): ", Kd); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Kd = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; + break; + case 5: + bt.printf("Current: %f, New (5 digits): ", Ks); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Ks = atof(in); + cmd = 'q'; + break; + case 6: + bt.printf("Current: %f, New (5 digits): ", ref_v); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + ref_v = atof(in); + motor_ctrl.setSetPoint(ref_v); + cmd = 'q'; + break; +// case 7: +// while (lock == 0); +// for (int i = 0; i < 128; i++) { +// bt.printf("%d, ", img[i]); +// } +// bt.printf("%d\r\n", center); +// lock = 0; +// break; + } + if (bt.readable()) { + cmd = bt.getc(); + } + } } } -// Communications -//void communication(void const *args) { -// while (true) { -// bt.printf("\r\nPress q to return to this prompt.\r\n"); -// bt.printf("Available diagnostics:\r\n"); -// bt.printf(" [0] Velocity\r\n"); -// bt.printf(" [1] Steering\r\n"); -// bt.printf(" [2] Change Kp\r\n"); -// bt.printf(" [3] Change Ki\r\n"); -// bt.printf(" [4] Change Kd\r\n"); -// bt.printf(" [5] Change Ks\r\n"); -// bt.printf(" [6] Change reference velocity\r\n"); -// cmd = bt.getc(); -// while (cmd != 'q') { -// switch(atoi(&cmd)){ -// case 0: -// bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", d, curr_pulses, velocity, Kp, Ki, Kd); -// break; -// case 1: -// bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); -// break; -// case 2: -// bt.printf("Current: %f, New (5 digits): ", Kp); -// for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); -// } -// bt.printf("\r\n"); -// Kp = atof(in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; -// break; -// case 3: -// bt.printf("Current: %f, New (5 digits): ", Ki); -// for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); -// } -// bt.printf("\r\n"); -// Ki = atof(in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; -// break; -// case 4: -// bt.printf("Current: %f, New (5 digits): ", Kd); -// for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); -// } -// bt.printf("\r\n"); -// Kd = atof(in); -// motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; -// break; -// case 5: -// bt.printf("Current: %f, New (5 digits): ", Ks); -// for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); -// } -// bt.printf("\r\n"); -// Ks = atof(in); -// cmd = 'q'; -// break; -// case 6: -// bt.printf("Current: %f, New (5 digits): ", ref_v); -// for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); -// } -// bt.printf("\r\n"); -// ref_v = atof(in); -// motor_ctrl.setSetPoint(ref_v); -// cmd = 'q'; -// break; -// } -// if (bt.readable()) { -// cmd = bt.getc(); -// } -// } -// } -//} - // Read data from camera void read_camera() { // Start data transfer + //if (lock == 1) { +// camera_read.attach_us(&read_camera, t_int); +// return; +// } si = 1; wait_us(1); clk = 1; @@ -205,7 +207,6 @@ for (int i = 0; i < 128; i++) { clk = 0; img[i] = ao.read_u16(); - tele_linescan[i] = img[i]; clk = 1; wait_us(1); } @@ -272,20 +273,18 @@ if (max > 43253) { center = (argmax + argmin + 2 + 11) / 2; - tele_center = center; a = 88 + (64 - center) * Ks; servo = a / 180; } camera_read.attach_us(&read_camera, t_int); +// lock = 1; } // ==== // Main // ==== int main() { - osThreadSetPriority(osThreadGetId(), osPriorityRealTime); - t.start(); // Initialize motor motor.period_us(T); @@ -307,31 +306,19 @@ motor_ctrl.setMode(1); // Initialize bluetooth -// bt.baud(115200); - -// osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal); + bt.baud(115200); // Initialize communications thread -// Thread communication_thread(communication); - Thread tele_comm_thread(tele_comm); -// tele_comm_thread.set_priority(osPriorityBelowNormal); + Thread communication_thread(communication); // Start motor controller while (true) { -// telemetry_serial.printf("%d\r\n", t.read_ms()); -// if (dec == 100) { - //tele_time_ms = t.read_ms(); -// telemetry_obj.do_io(); -// dec = 0; -// } -// dec = dec + 1; curr_pulses = qei.getPulses(); //for (int i = 0; i < MVG_AVG - 1; i++) { // v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1])); // } // v_prev[MVG_AVG-1] = velocity; velocity = (curr_pulses - prev_pulses)/ interval / ppr * c / 2.5; - tele_vel = velocity; //vel = velocity; // for (int i = 0; i < MVG_AVG; i++) { // velocity = velocity + v_prev[i]; @@ -341,7 +328,7 @@ motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; - Thread::wait(interval*1000); -// pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); + wait(interval); + pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); } } \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/printbuf.c Tue Apr 05 18:30:41 2016 +0000 @@ -0,0 +1,65 @@ +/***********************************************************************/ +/* */ +/* printbuf.c: Low Level print ring buffer Routines */ +/* */ +/***********************************************************************/ +#include <stdio.h> +#include <stdarg.h> + +#define CR 0x0D +#define TEMT 0x40 /* serial COMTX empty */ +extern int write (int file, char * ptr, int len); +int putchar_buf(int); +int putcharNB(int); + + +/* Background non-blocking print using ring buffer */ +#define PRNBUFSZ 0x100 +char printbuffer[PRNBUFSZ]; +int prnbuf_count = 0; /* number of characters in buffer */ +int prnbuf_pos = 0; /* location to store characters */ + + +int printfNB(const char *format, ...) +{ char buffer[128]; int i; + char c; + int val; + va_list args; + va_start( args, format ); + val = vsprintf(buffer, format, args ); // print to string using variable arguments + for(i = 0; i < 128 && buffer[i]!= '\0'; i++) // copy string to print buffer + { putcharNB((int)buffer[i]); } + return(val); +} + + +// overload putchar to use non-blocking print to ring buffer instead +int putcharNB(int c) { + if (c == '\n') putchar_buf(CR); + putchar_buf(c); + return(c); } + + +/* routine to print using print ring buffer */ +/* does not block - allows over runs */ +int putchar_buf(int c) +{ + if(prnbuf_count >= PRNBUFSZ) return(0); /* no room - drop character */ + printbuffer[prnbuf_pos] = c; + prnbuf_pos++; + prnbuf_count++; /* need to make uninterruptable? */ + if(prnbuf_pos > (PRNBUFSZ-1)) prnbuf_pos = 0; /* wrap index to beginning of + buffer */ + return(c); +} + + + + +int write (int file, char * ptr, int len) { + int i; + + for (i = 0; i < len; i++) putcharNB(*ptr++); + return len; +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/printf-stdarg.c Tue Apr 05 18:30:41 2016 +0000 @@ -0,0 +1,266 @@ +/* + Copyright 2001, 2002 Georges Menie (www.menie.org) + stdarg version contributed by Christian Ettinger + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* + putchar is the only external dependency for this file, + if you have a working putchar, leave it commented out. + If not, uncomment the define below and + replace outbyte(c) by your own function call. */ + +#define putchar(c) putcharNB(c) + + +#include <stdarg.h> + +static void printchar(char **str, int c) +{ + extern int putchar(int c); + + if (str) { + **str = c; + ++(*str); + } + else (void)putchar(c); +} + +#define PAD_RIGHT 1 +#define PAD_ZERO 2 + +static int prints(char **out, const char *string, int width, int pad) +{ + register int pc = 0, padchar = ' '; + + if (width > 0) { + register int len = 0; + register const char *ptr; + for (ptr = string; *ptr; ++ptr) ++len; + if (len >= width) width = 0; + else width -= len; + if (pad & PAD_ZERO) padchar = '0'; + } + if (!(pad & PAD_RIGHT)) { + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + } + for ( ; *string ; ++string) { + printchar (out, *string); + ++pc; + } + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + + return pc; +} + +/* the following should be enough for 32 bit int */ +#define PRINT_BUF_LEN 12 + +static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +{ + char print_buf[PRINT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; + + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints (out, print_buf, width, pad); + } + + if (sg && b == 10 && i < 0) { + neg = 1; + u = -i; + } + + s = print_buf + PRINT_BUF_LEN-1; + *s = '\0'; + + while (u) { + t = u % b; + if( t >= 10 ) + t += letbase - '0' - 10; + *--s = t + '0'; + u /= b; + } + + if (neg) { + if( width && (pad & PAD_ZERO) ) { + printchar (out, '-'); + ++pc; + --width; + } + else { + *--s = '-'; + } + } + + return pc + prints (out, s, width, pad); +} + +static int print(char **out, const char *format, va_list args ) +{ + register int width, pad; + register int pc = 0; + char scr[2]; + + for (; *format != 0; ++format) { + if (*format == '%') { + ++format; + width = pad = 0; + if (*format == '\0') break; + if (*format == '%') goto out; + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + for ( ; *format >= '0' && *format <= '9'; ++format) { + width *= 10; + width += *format - '0'; + } + if( *format == 's' ) { + register char *s = (char *)va_arg( args, int ); + pc += prints (out, s?s:"(null)", width, pad); + continue; + } + if( *format == 'd' ) { + pc += printi (out, va_arg( args, int ), 10, 1, width, pad, 'a'); + continue; + } + if( *format == 'x' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'a'); + continue; + } + if( *format == 'X' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'A'); + continue; + } + if( *format == 'u' ) { + pc += printi (out, va_arg( args, int ), 10, 0, width, pad, 'a'); + continue; + } + if( *format == 'c' ) { + /* char are converted to int then pushed on the stack */ + scr[0] = (char)va_arg( args, int ); + scr[1] = '\0'; + pc += prints (out, scr, width, pad); + continue; + } + } + else { + out: + printchar (out, *format); + ++pc; + } + } + if (out) **out = '\0'; + va_end( args ); + return pc; +} + +int printfNB(const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( 0, format, args ); +} + +int sprintf(char *out, const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( &out, format, args ); +} + +#ifdef TEST_PRINTF +int main(void) +{ + char *ptr = "Hello world!"; + char *np = 0; + int i = 5; + unsigned int bs = sizeof(int)*8; + int mi; + char buf[80]; + + mi = (1 << (bs-1)) + 1; + printf("%s\n", ptr); + printf("printf test\n"); + printf("%s is null pointer\n", np); + printf("%d = 5\n", i); + printf("%d = - max int\n", mi); + printf("char %c = 'a'\n", 'a'); + printf("hex %x = ff\n", 0xff); + printf("hex %02x = 00\n", 0); + printf("signed %d = unsigned %u = hex %x\n", -3, -3, -3); + printf("%d %s(s)%", 0, "message"); + printf("\n"); + printf("%d %s(s) with %%\n", 0, "message"); + sprintf(buf, "justif: \"%-10s\"\n", "left"); printf("%s", buf); + sprintf(buf, "justif: \"%10s\"\n", "right"); printf("%s", buf); + sprintf(buf, " 3: %04d zero padded\n", 3); printf("%s", buf); + sprintf(buf, " 3: %-4d left justif.\n", 3); printf("%s", buf); + sprintf(buf, " 3: %4d right justif.\n", 3); printf("%s", buf); + sprintf(buf, "-3: %04d zero padded\n", -3); printf("%s", buf); + sprintf(buf, "-3: %-4d left justif.\n", -3); printf("%s", buf); + sprintf(buf, "-3: %4d right justif.\n", -3); printf("%s", buf); + + return 0; +} + +/* + * if you compile this file with + * gcc -Wall $(YOUR_C_OPTIONS) -DTEST_PRINTF -c printf.c + * you will get a normal warning: + * printf.c:214: warning: spurious trailing `%' in format + * this line is testing an invalid % at the end of the format string. + * + * this should display (on 32bit int machine) : + * + * Hello world! + * printf test + * (null) is null pointer + * 5 = 5 + * -2147483647 = - max int + * char a = 'a' + * hex ff = ff + * hex 00 = 00 + * signed -3 = unsigned 4294967293 = hex fffffffd + * 0 message(s) + * 0 message(s) with % + * justif: "left " + * justif: " right" + * 3: 0003 zero padded + * 3: 3 left justif. + * 3: 3 right justif. + * -3: -003 zero padded + * -3: -3 left justif. + * -3: -3 right justif. + */ + +#endif