EE192 Team 4
Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
Sequential_Timing
by 
EE192 Team 4
Revision 0:fcf070a88ba0, committed 2016-03-18
- Comitter:
- vsutardja
- Date:
- Fri Mar 18 22:33:47 2016 +0000
- Child:
- 1:32610c005260
- Commit message:
- Figure 8 checkpoint working
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastAnalogIn.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/FastAnalogIn/#afc3b84dbbd6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#d8422efe4761
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 18 22:33:47 2016 +0000
@@ -0,0 +1,245 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "Servo.h"
+#include "FastAnalogIn.h"
+#include "PID.h"
+#include "QEI.h"
+#include "telemetry.h"
+
+// =========
+// Telemetry
+// =========
+//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
+//int idx = 0;
+char cmd; // Command
+char ch;
+char in[2];
+
+void communication(void const *args); // Communications
+
+// =====
+// Motor
+// =====
+PwmOut motor(PTA4); // Enable pin (PWM)
+int T = 25000; // Frequency
+float d = 0.0; // Duty cycle
+
+// =======
+// Encoder
+// =======
+int ppr = 389; // Pulses per revolution
+QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
+float c = 0.20106; // Wheel circumference
+int prev_pulses = 0; // Previous pulse count
+int curr_pulses = 0; // Current pulse count
+float velocity = 0; // Velocity
+
+// ========
+// Velocity
+// ========
+float Kp = 3.0; // Proportional factor
+float Ki = 0; // Integral factor
+float Kd = 0; // Derivative factor
+float interval = 0.01; // Sampling interval
+PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
+
+// =====
+// Servo
+// =====
+Servo servo(PTA12); // Enable pin (PWM)
+float a = 88; // Angle
+
+// ======
+// Camera
+// ======
+DigitalOut clk(PTD5); // Clock pin
+DigitalOut si(PTD0); // SI pin
+FastAnalogIn ao(PTC2); // AO pin
+Timeout camera_read; // Camera read timeout
+int t_int = 15000; // Exposure time
+int img[128]; // Image data
+
+void readCamera(); // Read data from camera
+
+// ================
+// Image processing
+// ================
+int max = -1;
+int argmax = 0;
+int argmin = 0;
+int temp[128];
+int center;
+
+void track(); // Line-tracking steering
+
+// ================
+// Functions
+// ================
+
+// 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 exposure time\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\r\n", d, curr_pulses, velocity, Kp);
+ break;
+ case 1:
+ bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int);
+ break;
+ case 2:
+// idx = 0;
+ bt.printf("Current Kp = %f. Enter new Kp: ", Kp);
+// while ((cmd = bt.getc()) != '\n') {
+ for (int idx = 0; idx < 2; idx++) {
+ ch = bt.getc();
+ in[idx] = ch;
+// idx = idx + 1;
+ }
+ bt.printf("\r\n");
+ Kp = atof(in);
+ break;
+ case 3:
+// idx = 0;
+ bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000);
+// while ((cmd = bt.getc()) != '\n') {
+ for (int idx = 0; idx < 2; idx++) {
+ ch = bt.getc();
+ in[idx] = ch;
+// idx = idx + 1;
+ }
+ bt.printf("\r\n");
+ t_int = atoi(in);
+ break;
+ }
+ if (bt.readable()) {
+ cmd = bt.getc();
+ }
+ }
+ }
+}
+
+// Read data from camera
+void read_camera() {
+ // Start data transfer
+ si = 1;
+ wait_us(1);
+ clk = 1;
+ wait_us(1);
+ si = 0;
+ wait_us(1);
+
+ // Read camera data
+ for (int i = 0; i < 128; i++) {
+ clk = 0;
+ img[i] = ao.read_u16();
+ clk = 1;
+ wait_us(1);
+ }
+ clk = 0;
+
+ // Update servo angle
+ track();
+
+ // Set next frame exposure time
+ camera_read.attach_us(&read_camera, t_int);
+}
+
+// Find line center from image
+// Take two-point moving average to smooth the data
+// Find indices where max and min of smoothed data are attained
+// Calculate and return midpoint of argmax and argmin
+void track() {
+ max = -1;
+ argmax = 0;
+ argmin = 0;
+ for (int i = 0; i < 107; i++) {
+ if (img[i+11] > max) {
+ max = img[i+11];
+ }
+ if (i == 126) {
+ temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1];
+ if (temp[i-1] > temp[argmax]) {
+ argmax = i - 1;
+ }
+ if (temp[i-1] < temp[argmin]) {
+ argmin = i - 1;
+ }
+ } else {
+ temp[i] = (img[i+11] + img[i+1+11]) / 2;
+ if (i > 0) {
+ temp[i-1] = temp[i] - temp[i-1];
+ if (temp[i-1] > temp[argmax]) {
+ argmax = i - 1;
+ }
+ if (temp[i-1] < temp[argmin]) {
+ argmin = i - 1;
+ }
+ }
+ }
+ }
+
+ if (max > 43253) {
+ center = (argmax + argmin + 2 + 11) / 2;
+ a = 88 + (64 - center) * 0.625;
+ servo = a / 180;
+ }
+}
+
+// ====
+// Main
+// ====
+int main() {
+ // Initialize motor
+ motor.period_us(T);
+ motor = 1.0 - d;
+
+ // Initialize servo
+ servo.calibrate(0.001, 45.0);
+ servo = a / 180.0;
+
+ // Initialize & start camera
+ clk = 0;
+ read_camera();
+
+ // Initialize motor controller
+ motor_ctrl.setInputLimits(0.0, 10.0);
+ motor_ctrl.setOutputLimits(0.1, 0.5);
+ motor_ctrl.setSetPoint(3.5);
+ motor_ctrl.setBias(0.0);
+ motor_ctrl.setMode(1);
+
+ // Initialize bluetooth
+ bt.baud(115200);
+
+ // Initialize communications thread
+ Thread communication_thread(communication);
+
+ // Start motor controller
+ while (true) {
+ curr_pulses = qei.getPulses();
+ velocity = (curr_pulses - prev_pulses)/ interval / ppr * c;
+ prev_pulses = curr_pulses;
+ motor_ctrl.setProcessValue(velocity);
+ d = motor_ctrl.compute();
+ motor = 1.0 - d;
+ wait(interval);
+ pc.printf("Velocity: %f\r\n", velocity);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#bdd541595fc5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/c0f6e94411f5 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/telemetry.lib Fri Mar 18 22:33:47 2016 +0000 @@ -0,0 +1,1 @@ +telemetry#aca5a32d2759