Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of FixedPWMWill by
Revision 11:4348bba086a4, committed 2016-04-05
- Comitter:
- vsutardja
- Date:
- Tue Apr 05 21:52:32 2016 +0000
- Parent:
- 10:716484b1ddb5
- Child:
- 12:54e7d8ff3a74
- Commit message:
- Telemetry cont'd
Changed in this revision
--- a/main.cpp Tue Apr 05 18:30:41 2016 +0000 +++ b/main.cpp Tue Apr 05 21:52:32 2016 +0000 @@ -6,29 +6,33 @@ #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 +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); +telemetry::Numeric<uint32_t> tele_t_int(telemetry_obj, "t_t_int", "t_int", "us", 0); +telemetry::Numeric<uint32_t> tele_cam_dec(telemetry_obj, "t_cam_dec", "decimation", "", 0); // ============= // Communication // ============= Serial pc(USBTX, USBRX); // USB connection -MODSERIAL bt(PTC4, PTC3); // BlueSMiRF connection -//int idx = 0; +//Serial bt(PTC4, PTC3); // BlueSMiRF connection char cmd; // Command char ch; char in[5]; void communication(void const *args); // Communications -int lock = 0; // ===== // Motor @@ -56,7 +60,7 @@ float Kp = 6.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor -float interval = 0.01; // Sampling interval +float interval = 0.1; // Sampling interval float ref_v = 1.0; PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller @@ -65,7 +69,7 @@ // ===== Servo servo(PTA12); // Enable pin (PWM) float a = 88; // Angle -float Ks = 0.7; +float Ks = 0.9; // ====== // Camera @@ -74,9 +78,11 @@ DigitalOut si(PTD0); // SI pin FastAnalogIn ao(PTC2); // AO pin Timeout camera_read; // Camera read timeout +const int T_INT_MIN = 2500; +int cam_dec = 0; +int cam_dec_count = 0; int t_int = 15000; // Exposure time int img[128]; // Image data -uint8_t img_out[128]; void readCamera(); // Read data from camera @@ -97,105 +103,122 @@ // Functions // ================ -// Communications -void communication(void const *args) { +void tele_comm(void const *args) { + telemetry_serial.baud(115200); + telemetry_obj.transmit_header(); 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"); - 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(); - } - } + tele_time_ms = t.read_ms(); + telemetry_obj.do_io(); + Thread::wait(100); } } +// 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) { + //if (cam_dec_count < cam_dec) { +// si = 1; +// wait_us(1); +// clk = 1; +// wait_us(1); +// si = 0; +// +// for (int i = 0; i < 128; i++) { +// wait_us(1); +// clk = 0; +// wait_us(1); +// clk = 1; +// } +// clk = 0; +// cam_dec_count = cam_dec_count + 1; // camera_read.attach_us(&read_camera, t_int); // return; // } + cam_dec_count = 0; + + // Start data transfer si = 1; wait_us(1); clk = 1; @@ -207,6 +230,7 @@ for (int i = 0; i < 128; i++) { clk = 0; img[i] = ao.read_u16(); + tele_linescan[i] = img[i]; clk = 1; wait_us(1); } @@ -255,36 +279,64 @@ } for (int i = 0; i < 10; i++) { - lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0; + lum_bg = lum_bg + img[55 - 4 - i] / 20.0 + img[55 + 4 + i] / 20.0; } contrast = (max - lum_bg) / lum_bg; // if (contrast < 1.5) { - // Underexposed - if (max < 60000) { - t_int = t_int + 0.15 * (60000 - max); - } - // Overexposed - if (lum_bg > 25000) { - t_int = t_int - 0.15 * (lum_bg - 25000); - } + // Underexposed + //if (max < 60000) { +// t_int = t_int + 0.15 * (60000 - max); +// } +// // Overexposed +// if (lum_bg > 25000) { +// t_int = t_int - 0.15 * (lum_bg - 25000); +// } +// } + + if (max > 60000) { + t_int = t_int - 0.1 * (max - 60000); + } + if (max < 50000) { + t_int = t_int + 0.1 * (50000 - max); + } + + if (t_int < 1000) { + t_int = 1000; + } + + tele_t_int = t_int; + + //if (t_int < T_INT_MIN) { +// cam_dec = T_INT_MIN / t_int; // } - if (max > 43253) { + tele_cam_dec = cam_dec; + + if (max > 43253 && argmax < argmin) { center = (argmax + argmin + 2 + 11) / 2; - a = 88 + (64 - center) * Ks; + tele_center = center; + a = 88 + (55 - center) * Ks; + if (a > 113) { + a = 113; + } + if (a < 63) { + a = 63; + } servo = a / 180; } +// camera_read.attach_us(&read_camera, 1000); camera_read.attach_us(&read_camera, t_int); -// lock = 1; } // ==== // Main // ==== int main() { +// osThreadSetPriority(osThreadGetId(), osPriorityRealTime); + t.start(); // Initialize motor motor.period_us(T); @@ -306,19 +358,31 @@ motor_ctrl.setMode(1); // Initialize bluetooth - bt.baud(115200); +// bt.baud(115200); + +// osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal); // Initialize communications thread - Thread communication_thread(communication); +// Thread communication_thread(communication); + Thread tele_comm_thread(tele_comm); +// tele_comm_thread.set_priority(osPriorityBelowNormal); // 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]; @@ -328,7 +392,7 @@ motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; - wait(interval); - pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); + Thread::wait(interval*1000); +// pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); } } \ No newline at end of file
--- a/printbuf.c Tue Apr 05 18:30:41 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,65 +0,0 @@ -/***********************************************************************/ -/* */ -/* 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; -} -
--- a/printf-stdarg.c Tue Apr 05 18:30:41 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,266 +0,0 @@ -/* - 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