Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing 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
