EE192 Team 4 / Mbed 2 deprecated FixedPWM

Fixed PWM

Dependencies:   FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry

Fork of Sequential_Timing by EE192 Team 4

main.cpp@18:2b7db50fec4c, 2016-04-14 (annotated)

Committer:
vsutardja
Date:
Thu Apr 14 01:44:28 2016 +0000
Revision:
18:2b7db50fec4c
Parent:
17:bf6192a361ab 
fixed pwm, pid steering

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsutardja 12:54e7d8ff3a74 1 #include "FastAnalogIn.h"
vsutardja 0:fcf070a88ba0 2 #include "mbed.h"
vsutardja 0:fcf070a88ba0 3 #include "PID.h"
vsutardja 0:fcf070a88ba0 4 #include "QEI.h"
vsutardja 12:54e7d8ff3a74 5 #include "rtos.h"
vsutardja 12:54e7d8ff3a74 6 #include "SerialRPCInterface.h"
vsutardja 12:54e7d8ff3a74 7 #include "Servo.h"
vsutardja 0:fcf070a88ba0 8 #include "telemetry.h"
vsutardja 0:fcf070a88ba0 9
vsutardja 12:54e7d8ff3a74 10
vsutardja 0:fcf070a88ba0 11 // =========
vsutardja 0:fcf070a88ba0 12 // Telemetry
vsutardja 0:fcf070a88ba0 13 // =========
vsutardja 18:2b7db50fec4c 14 //MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
vsutardja 18:2b7db50fec4c 15 MODSERIAL telemetry_serial(USBTX, USBRX);
vsutardja 12:54e7d8ff3a74 16 telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
vsutardja 12:54e7d8ff3a74 17 telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
vsutardja 11:4348bba086a4 18
vsutardja 11:4348bba086a4 19 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
vsutardja 12:54e7d8ff3a74 20 telemetry::NumericArray<uint16_t, 108> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
vsutardja 12:54e7d8ff3a74 21 telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0);
vsutardja 12:54e7d8ff3a74 22 telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);
vsutardja 12:54e7d8ff3a74 23 telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0);
vsutardja 12:54e7d8ff3a74 24 telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
vsutardja 0:fcf070a88ba0 25
vsutardja 12:54e7d8ff3a74 26 Timer t;
vsutardja 12:54e7d8ff3a74 27 Timer t_tele;
vsutardja 12:54e7d8ff3a74 28 Ticker control_interrupt;
vsutardja 18:2b7db50fec4c 29 int t_cam = 0;
vsutardja 12:54e7d8ff3a74 30 int t_steer = 0;
vsutardja 12:54e7d8ff3a74 31 int t_vel = 0;
vsutardja 12:54e7d8ff3a74 32
vsutardja 14:a0614f48e6ef 33 float interrupt_T = 0.015;
vsutardja 14:a0614f48e6ef 34 bool ctrl_flag = false;
vsutardja 0:fcf070a88ba0 35
vsutardja 0:fcf070a88ba0 36 // =============
vsutardja 0:fcf070a88ba0 37 // Communication
vsutardja 0:fcf070a88ba0 38 // =============
vsutardja 12:54e7d8ff3a74 39 char comm_cmd; // Command
vsutardja 18:2b7db50fec4c 40 char comm_in[8]; // Input
vsutardja 18:2b7db50fec4c 41 //Serial bt(USBTX, USBRX); // USB connection
vsutardja 18:2b7db50fec4c 42 Serial bt(PTC4, PTC3); // BlueSMiRF connection
vsutardja 0:fcf070a88ba0 43
vsutardja 12:54e7d8ff3a74 44 void communication(void const *args); // Communications
vsutardja 0:fcf070a88ba0 45
vsutardja 18:2b7db50fec4c 46 //void Kmill(Arguments *input, Reply *output);
vsutardja 18:2b7db50fec4c 47 //RPCFunction rpc_Kmill(&Kmill, "Kmill");
vsutardja 18:2b7db50fec4c 48
vsutardja 0:fcf070a88ba0 49 // =====
vsutardja 0:fcf070a88ba0 50 // Motor
vsutardja 0:fcf070a88ba0 51 // =====
vsutardja 12:54e7d8ff3a74 52 const int motor_T = 25000; // Frequency
vsutardja 12:54e7d8ff3a74 53 float motor_duty = 0.0; // Duty cycle
vsutardja 12:54e7d8ff3a74 54 bool e_stop = false; // Emergency stop
vsutardja 12:54e7d8ff3a74 55 PwmOut motor(PTA4); // Enable pin (PWM)
vsutardja 0:fcf070a88ba0 56
vsutardja 0:fcf070a88ba0 57 // =======
vsutardja 0:fcf070a88ba0 58 // Encoder
vsutardja 0:fcf070a88ba0 59 // =======
vsutardja 12:54e7d8ff3a74 60 const int ppr = 389; // Pulses per revolution
vsutardja 12:54e7d8ff3a74 61 const float c = 0.20106; // Wheel circumference
vsutardja 12:54e7d8ff3a74 62 int prev_pulses = 0; // Previous pulse count
vsutardja 12:54e7d8ff3a74 63 int curr_pulses = 0; // Current pulse count
vsutardja 12:54e7d8ff3a74 64 float velocity = 0; // Velocity
vsutardja 12:54e7d8ff3a74 65 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
vsutardja 0:fcf070a88ba0 66
vsutardja 0:fcf070a88ba0 67 // ========
vsutardja 0:fcf070a88ba0 68 // Velocity
vsutardja 0:fcf070a88ba0 69 // ========
vsutardja 18:2b7db50fec4c 70 float Kmp = 8.0; // Proportional factor
vsutardja 18:2b7db50fec4c 71 float Kmi = 0; // Integral factor
vsutardja 18:2b7db50fec4c 72 float Kmd = 0; // Derivative factor
vsutardja 17:bf6192a361ab 73 float interval = 0.01; // Sampling interval
vsutardja 18:2b7db50fec4c 74 float prev_vels[10];
vsutardja 18:2b7db50fec4c 75 float ref_v = 0.8; // Reference velocity
vsutardja 18:2b7db50fec4c 76 PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
vsutardja 0:fcf070a88ba0 77
vsutardja 0:fcf070a88ba0 78 // =====
vsutardja 0:fcf070a88ba0 79 // Servo
vsutardja 0:fcf070a88ba0 80 // =====
vsutardja 12:54e7d8ff3a74 81 float angle = 88; // Angle
vsutardja 18:2b7db50fec4c 82 float Ksp = 0.9; // Steering proportion
vsutardja 16:3ab3c4670f4f 83 float Ksi = 0;
vsutardja 16:3ab3c4670f4f 84 float Ksd = 0;
vsutardja 16:3ab3c4670f4f 85 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
vsutardja 12:54e7d8ff3a74 86 Servo servo(PTA12); // Signal pin (PWM)
vsutardja 0:fcf070a88ba0 87
vsutardja 0:fcf070a88ba0 88 // ======
vsutardja 0:fcf070a88ba0 89 // Camera
vsutardja 0:fcf070a88ba0 90 // ======
vsutardja 12:54e7d8ff3a74 91 int t_int = 10000; // Exposure time
vsutardja 18:2b7db50fec4c 92 const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
vsutardja 12:54e7d8ff3a74 93 const int T_INT_MIN = 35; // Minimum exposure time
vsutardja 12:54e7d8ff3a74 94 int img[108] = {0}; // Image data
vsutardja 12:54e7d8ff3a74 95 DigitalOut clk(PTD5); // Clock pin
vsutardja 12:54e7d8ff3a74 96 DigitalOut si(PTD0); // SI pin
vsutardja 12:54e7d8ff3a74 97 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 98
vsutardja 0:fcf070a88ba0 99 // ================
vsutardja 0:fcf070a88ba0 100 // Image processing
vsutardja 0:fcf070a88ba0 101 // ================
vsutardja 12:54e7d8ff3a74 102 int max = -1; // Maximum luminosity
vsutardja 12:54e7d8ff3a74 103 int left[5] = {0}; // Left edge
vsutardja 12:54e7d8ff3a74 104 int right[5] = {0}; // Right edge
vsutardja 12:54e7d8ff3a74 105 int j = 0; // Peaks index
vsutardja 12:54e7d8ff3a74 106 int center = 64; // Center estimate
vsutardja 12:54e7d8ff3a74 107 int centers[5] = {0}; // Possible centers
vsutardja 12:54e7d8ff3a74 108 int prev_center = 64; // Previous center
vsutardja 12:54e7d8ff3a74 109 float scores[5] = {0}; // Candidate scores
vsutardja 12:54e7d8ff3a74 110 int best_score_idx = 0; // Most likely center index
vsutardja 0:fcf070a88ba0 111
vsutardja 0:fcf070a88ba0 112 // ================
vsutardja 0:fcf070a88ba0 113 // Functions
vsutardja 0:fcf070a88ba0 114 // ================
vsutardja 0:fcf070a88ba0 115
vsutardja 18:2b7db50fec4c 116
vsutardja 18:2b7db50fec4c 117 void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
vsutardja 13:d6e167698517 118 MODSERIAL *serial = q->serial;
vsutardja 13:d6e167698517 119 if (serial->rxGetLastChar() == 'k') {
vsutardja 13:d6e167698517 120 e_stop = true;
vsutardja 13:d6e167698517 121 motor = 1.0;
vsutardja 13:d6e167698517 122 }
vsutardja 15:db95bb7c7f93 123 if (serial->rxGetLastChar() == '+') {
vsutardja 18:2b7db50fec4c 124 ref_v = ref_v + 0.05;
vsutardja 13:d6e167698517 125 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 126 }
vsutardja 13:d6e167698517 127 if (serial->rxGetLastChar() == '-') {
vsutardja 18:2b7db50fec4c 128 ref_v = ref_v - 0.05;
vsutardja 13:d6e167698517 129 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 130 }
vsutardja 13:d6e167698517 131 }
vsutardja 13:d6e167698517 132
vsutardja 12:54e7d8ff3a74 133 // Communications
vsutardja 11:4348bba086a4 134 //void communication(void const *args) {
vsutardja 14:a0614f48e6ef 135 // telemetry_serial.baud(115200);
vsutardja 18:2b7db50fec4c 136 // telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
vsutardja 14:a0614f48e6ef 137 // telemetry_obj.transmit_header();
vsutardja 11:4348bba086a4 138 // while (true) {
vsutardja 14:a0614f48e6ef 139 // tele_time_ms = t_tele.read_ms();
vsutardja 14:a0614f48e6ef 140 // telemetry_obj.do_io();
vsutardja 11:4348bba086a4 141 // }
vsutardja 11:4348bba086a4 142 //}
vsutardja 11:4348bba086a4 143
vsutardja 14:a0614f48e6ef 144 void communication(void const *args) {
vsutardja 14:a0614f48e6ef 145 // Initialize bluetooth
vsutardja 14:a0614f48e6ef 146 bt.baud(115200);
vsutardja 14:a0614f48e6ef 147
vsutardja 14:a0614f48e6ef 148 while (true) {
vsutardja 14:a0614f48e6ef 149 bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 14:a0614f48e6ef 150 bt.printf("Available diagnostics:\r\n");
vsutardja 14:a0614f48e6ef 151 bt.printf(" [0] Velocity\r\n");
vsutardja 14:a0614f48e6ef 152 bt.printf(" [1] Steering\r\n");
vsutardja 18:2b7db50fec4c 153 // bt.printf(" [2] Change Kmp\r\n");
vsutardja 18:2b7db50fec4c 154 // bt.printf(" [3] Change Kmi\r\n");
vsutardja 18:2b7db50fec4c 155 // bt.printf(" [4] Change Kmd\r\n");
vsutardja 18:2b7db50fec4c 156 bt.printf(" [2] Change Ksp\r\n");
vsutardja 18:2b7db50fec4c 157 bt.printf(" [3] Change Ksi\r\n");
vsutardja 18:2b7db50fec4c 158 bt.printf(" [4] Change Ksd\r\n");
vsutardja 16:3ab3c4670f4f 159 bt.printf(" [5] Change Ksp\r\n");
vsutardja 14:a0614f48e6ef 160 bt.printf(" [6] Change reference velocity\r\n");
vsutardja 14:a0614f48e6ef 161 bt.printf(" [7] EMERGENCY STOP\r\n");
vsutardja 18:2b7db50fec4c 162 // bt.printf(" [8] Timing\r\n");
vsutardja 18:2b7db50fec4c 163 bt.printf(" [8] duty += 0.05\r\n");
vsutardja 18:2b7db50fec4c 164 bt.printf(" [9] duty -= 0.05\r\n");
vsutardja 14:a0614f48e6ef 165 comm_cmd = bt.getc();
vsutardja 14:a0614f48e6ef 166 while (comm_cmd != 'q') {
vsutardja 14:a0614f48e6ef 167 switch(atoi(&comm_cmd)){
vsutardja 14:a0614f48e6ef 168 case 0:
vsutardja 16:3ab3c4670f4f 169 bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
vsutardja 14:a0614f48e6ef 170 break;
vsutardja 14:a0614f48e6ef 171 case 1:
vsutardja 14:a0614f48e6ef 172 bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int);
vsutardja 14:a0614f48e6ef 173 break;
vsutardja 14:a0614f48e6ef 174 case 2:
vsutardja 18:2b7db50fec4c 175 bt.printf("Current: %f, New (8 digits): ", Ksp);
vsutardja 18:2b7db50fec4c 176 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 177 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 178 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 179 }
vsutardja 14:a0614f48e6ef 180 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 181 Ksp = atof(comm_in);
vsutardja 18:2b7db50fec4c 182 servo_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 14:a0614f48e6ef 183 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 184 break;
vsutardja 14:a0614f48e6ef 185 case 3:
vsutardja 18:2b7db50fec4c 186 bt.printf("Current: %f, New (8 digits): ", Ksi);
vsutardja 18:2b7db50fec4c 187 for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 188 comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 189 bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 190 }
vsutardja 18:2b7db50fec4c 191 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 192 Ksi = atof(comm_in);
vsutardja 18:2b7db50fec4c 193 motor_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 18:2b7db50fec4c 194 comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 195 break;
vsutardja 18:2b7db50fec4c 196 case 4:
vsutardja 18:2b7db50fec4c 197 bt.printf("Current: %f, New (8 digits): ", Ksd);
vsutardja 18:2b7db50fec4c 198 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 199 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 200 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 201 }
vsutardja 14:a0614f48e6ef 202 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 203 Ksd = atof(comm_in);
vsutardja 18:2b7db50fec4c 204 motor_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 14:a0614f48e6ef 205 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 206 break;
vsutardja 18:2b7db50fec4c 207 // case 2:
vsutardja 18:2b7db50fec4c 208 // bt.printf("Current: %f, New (8 digits): ", Kmp);
vsutardja 18:2b7db50fec4c 209 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 210 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 211 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 212 // }
vsutardja 18:2b7db50fec4c 213 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 214 // Kmp = atof(comm_in);
vsutardja 18:2b7db50fec4c 215 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 216 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 217 // break;
vsutardja 18:2b7db50fec4c 218 // case 3:
vsutardja 18:2b7db50fec4c 219 // bt.printf("Current: %f, New (8 digits): ", Kmi);
vsutardja 18:2b7db50fec4c 220 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 221 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 222 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 223 // }
vsutardja 18:2b7db50fec4c 224 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 225 // Kmi = atof(comm_in);
vsutardja 18:2b7db50fec4c 226 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 227 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 228 // break;
vsutardja 18:2b7db50fec4c 229 // case 4:
vsutardja 18:2b7db50fec4c 230 // bt.printf("Current: %f, New (8 digits): ", Kmd);
vsutardja 18:2b7db50fec4c 231 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 232 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 233 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 234 // }
vsutardja 18:2b7db50fec4c 235 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 236 // Kmd = atof(comm_in);
vsutardja 18:2b7db50fec4c 237 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 238 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 239 // break;
vsutardja 14:a0614f48e6ef 240 case 5:
vsutardja 18:2b7db50fec4c 241 bt.printf("Current: %f, New (8 digits): ", Ksp);
vsutardja 18:2b7db50fec4c 242 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 243 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 244 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 245 }
vsutardja 14:a0614f48e6ef 246 bt.printf("\r\n");
vsutardja 16:3ab3c4670f4f 247 Ksp = atof(comm_in);
vsutardja 14:a0614f48e6ef 248 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 249 break;
vsutardja 14:a0614f48e6ef 250 case 6:
vsutardja 18:2b7db50fec4c 251 bt.printf("Current: %f, New (8 digits): ", ref_v);
vsutardja 18:2b7db50fec4c 252 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 253 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 254 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 255 }
vsutardja 14:a0614f48e6ef 256 bt.printf("\r\n");
vsutardja 14:a0614f48e6ef 257 ref_v = atof(comm_in);
vsutardja 14:a0614f48e6ef 258 motor_ctrl.setSetPoint(ref_v);
vsutardja 14:a0614f48e6ef 259 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 260 break;
vsutardja 14:a0614f48e6ef 261 case 7:
vsutardja 18:2b7db50fec4c 262 // e_stop = true;
vsutardja 18:2b7db50fec4c 263 motor = 1.0;
vsutardja 14:a0614f48e6ef 264 bt.printf("STOPPED\r\n");
vsutardja 14:a0614f48e6ef 265 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 266 break;
vsutardja 18:2b7db50fec4c 267 // case 8:
vsutardja 18:2b7db50fec4c 268 // bt.printf("Exposure: %dus, Camera Read: %dus, Steering: %dus, Velocity: %dus, Total: %dus\r\n", t_int, t_cam-t_int, t_steer, t_vel, t_cam+t_steer+t_vel);
vsutardja 18:2b7db50fec4c 269 // break;
vsutardja 14:a0614f48e6ef 270 case 8:
vsutardja 18:2b7db50fec4c 271 motor_duty = motor_duty + 0.05;
vsutardja 18:2b7db50fec4c 272 motor = 1.0 - motor_duty;
vsutardja 18:2b7db50fec4c 273 bt.printf("%f\r\n", motor_duty);
vsutardja 18:2b7db50fec4c 274 comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 275 break;
vsutardja 18:2b7db50fec4c 276 case 9:
vsutardja 18:2b7db50fec4c 277 motor_duty = motor_duty - 0.05;
vsutardja 18:2b7db50fec4c 278 motor = 1.0 - motor_duty;
vsutardja 18:2b7db50fec4c 279 bt.printf("%f\r\n", motor_duty);
vsutardja 18:2b7db50fec4c 280 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 281 break;
vsutardja 14:a0614f48e6ef 282 }
vsutardja 14:a0614f48e6ef 283 if (bt.readable()) {
vsutardja 14:a0614f48e6ef 284 comm_cmd = bt.getc();
vsutardja 14:a0614f48e6ef 285 }
vsutardja 14:a0614f48e6ef 286 }
vsutardja 14:a0614f48e6ef 287 }
vsutardja 14:a0614f48e6ef 288 }
vsutardja 14:a0614f48e6ef 289
vsutardja 12:54e7d8ff3a74 290 void control() {
vsutardja 12:54e7d8ff3a74 291 // Image capture
vsutardja 18:2b7db50fec4c 292 // t.reset();
vsutardja 12:54e7d8ff3a74 293
vsutardja 12:54e7d8ff3a74 294 // Dummy read
vsutardja 13:d6e167698517 295 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 296 PTD->PSOR = (0x01);
vsutardja 13:d6e167698517 297 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 298 PTD->PCOR = (0x01);
vsutardja 11:4348bba086a4 299
vsutardja 12:54e7d8ff3a74 300 for (int i = 0; i < 128; i++) {
vsutardja 13:d6e167698517 301 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 302 PTD->PSOR = (0x01 << 5);
vsutardja 12:54e7d8ff3a74 303 }
vsutardja 12:54e7d8ff3a74 304
vsutardja 12:54e7d8ff3a74 305 // Expose
vsutardja 12:54e7d8ff3a74 306 wait_us(t_int);
vsutardja 12:54e7d8ff3a74 307
vsutardja 12:54e7d8ff3a74 308 // Read camera
vsutardja 13:d6e167698517 309 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 310 PTD->PSOR = (0x01);
vsutardja 13:d6e167698517 311 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 312 PTD->PCOR = (0x01);
vsutardja 0:fcf070a88ba0 313
vsutardja 0:fcf070a88ba0 314 for (int i = 0; i < 128; i++) {
vsutardja 13:d6e167698517 315 PTD->PCOR = (0x01 << 5);
vsutardja 12:54e7d8ff3a74 316 if (i > 9 && i < 118) {
vsutardja 12:54e7d8ff3a74 317 img[i-10] = ao.read_u16();
vsutardja 12:54e7d8ff3a74 318 tele_linescan[i-10] = img[i-10];
vsutardja 12:54e7d8ff3a74 319 }
vsutardja 13:d6e167698517 320 PTD->PSOR = (0x01 << 5);
vsutardja 0:fcf070a88ba0 321 }
vsutardja 12:54e7d8ff3a74 322
vsutardja 18:2b7db50fec4c 323 // t_cam = t.read_us();
vsutardja 0:fcf070a88ba0 324
vsutardja 12:54e7d8ff3a74 325 // Steering control
vsutardja 18:2b7db50fec4c 326 // t.reset();
vsutardja 0:fcf070a88ba0 327
vsutardja 12:54e7d8ff3a74 328 // Detect peak edges
vsutardja 12:54e7d8ff3a74 329 j = 0;
vsutardja 18:2b7db50fec4c 330 for (int i = 0; i < 107 && j < 5; i++) {
vsutardja 18:2b7db50fec4c 331 if (img[i] > 45000) {
vsutardja 12:54e7d8ff3a74 332 left[j] = i;
vsutardja 18:2b7db50fec4c 333 while (img[i] > 45000) {
vsutardja 18:2b7db50fec4c 334 i = i + 1;
vsutardja 0:fcf070a88ba0 335 }
vsutardja 18:2b7db50fec4c 336 right[j] = i;
vsutardja 12:54e7d8ff3a74 337 j = j + 1;
vsutardja 0:fcf070a88ba0 338 }
vsutardja 0:fcf070a88ba0 339 }
vsutardja 0:fcf070a88ba0 340
vsutardja 18:2b7db50fec4c 341 // Calculate peak centers
vsutardja 18:2b7db50fec4c 342 for (int i = 0; i < j; i++) {
vsutardja 18:2b7db50fec4c 343 centers[i] = (left[i] + right[i] + 10) / 2;
vsutardja 18:2b7db50fec4c 344 }
vsutardja 12:54e7d8ff3a74 345
vsutardja 18:2b7db50fec4c 346 // Assign scores
vsutardja 18:2b7db50fec4c 347 for (int i = 0; i < j; i++) {
vsutardja 18:2b7db50fec4c 348 scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
vsutardja 18:2b7db50fec4c 349 }
vsutardja 18:2b7db50fec4c 350
vsutardja 18:2b7db50fec4c 351 // Choose most likely center
vsutardja 18:2b7db50fec4c 352 best_score_idx = 0;
vsutardja 18:2b7db50fec4c 353 for (int i = 0; i < j; i++) {
vsutardja 18:2b7db50fec4c 354 if (scores[i] > scores[best_score_idx]) {
vsutardja 18:2b7db50fec4c 355 best_score_idx = i;
vsutardja 12:54e7d8ff3a74 356 }
vsutardja 12:54e7d8ff3a74 357 }
vsutardja 18:2b7db50fec4c 358 prev_center = center;
vsutardja 18:2b7db50fec4c 359 center = centers[best_score_idx];
vsutardja 18:2b7db50fec4c 360 tele_center = center;
vsutardja 18:2b7db50fec4c 361
vsutardja 18:2b7db50fec4c 362 // Set servo angle
vsutardja 18:2b7db50fec4c 363 //angle = 88 + (55 - center) * Ksp;
vsutardja 18:2b7db50fec4c 364 // if (angle > 113) {
vsutardja 18:2b7db50fec4c 365 // angle = 113;
vsutardja 18:2b7db50fec4c 366 // }
vsutardja 18:2b7db50fec4c 367 // if (angle < 63) {
vsutardja 18:2b7db50fec4c 368 // angle = 63;
vsutardja 18:2b7db50fec4c 369 // }
vsutardja 18:2b7db50fec4c 370 // servo = angle / 180;
vsutardja 18:2b7db50fec4c 371 servo_ctrl.setProcessValue(center);
vsutardja 18:2b7db50fec4c 372 angle = 88 + servo_ctrl.compute();
vsutardja 18:2b7db50fec4c 373 servo = angle / 180;
vsutardja 12:54e7d8ff3a74 374
vsutardja 12:54e7d8ff3a74 375 // AGC
vsutardja 12:54e7d8ff3a74 376 max = -1;
vsutardja 12:54e7d8ff3a74 377 for (int i = 0; i < 107; i++) {
vsutardja 12:54e7d8ff3a74 378 if (img[i] > max) {
vsutardja 12:54e7d8ff3a74 379 max = img[i];
vsutardja 12:54e7d8ff3a74 380 }
vsutardja 12:54e7d8ff3a74 381 }
vsutardja 11:4348bba086a4 382 if (max > 60000) {
vsutardja 11:4348bba086a4 383 t_int = t_int - 0.1 * (max - 60000);
vsutardja 11:4348bba086a4 384 }
vsutardja 11:4348bba086a4 385 if (max < 50000) {
vsutardja 11:4348bba086a4 386 t_int = t_int + 0.1 * (50000 - max);
vsutardja 11:4348bba086a4 387 }
vsutardja 12:54e7d8ff3a74 388 if (t_int < T_INT_MIN) {
vsutardja 12:54e7d8ff3a74 389 t_int = T_INT_MIN;
vsutardja 12:54e7d8ff3a74 390 }
vsutardja 12:54e7d8ff3a74 391 if (t_int > T_INT_MAX) {
vsutardja 12:54e7d8ff3a74 392 t_int = T_INT_MAX;
vsutardja 11:4348bba086a4 393 }
vsutardja 12:54e7d8ff3a74 394 tele_exposure = t_int;
vsutardja 11:4348bba086a4 395
vsutardja 18:2b7db50fec4c 396 // t_steer = t.read_us();
vsutardja 11:4348bba086a4 397
vsutardja 18:2b7db50fec4c 398 // // Velocity control
vsutardja 18:2b7db50fec4c 399 // // t.reset();
vsutardja 18:2b7db50fec4c 400 // if (!e_stop) {
vsutardja 18:2b7db50fec4c 401 // curr_pulses = qei.getPulses();
vsutardja 18:2b7db50fec4c 402 // //for (int i = 0; i < 9; i++) {
vsutardja 18:2b7db50fec4c 403 //// prev_vels[i] = prev_vels[i+1];
vsutardja 18:2b7db50fec4c 404 //// }
vsutardja 18:2b7db50fec4c 405 //// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
vsutardja 18:2b7db50fec4c 406 //// t.reset();
vsutardja 18:2b7db50fec4c 407 //// if (prev_vels[9] < 0) {
vsutardja 18:2b7db50fec4c 408 //// prev_vels[9] = 0;
vsutardja 18:2b7db50fec4c 409 //// }
vsutardja 18:2b7db50fec4c 410 //// if (prev_vels[0] < 0) {
vsutardja 18:2b7db50fec4c 411 //// velocity = prev_vels[9];
vsutardja 18:2b7db50fec4c 412 //// } else {
vsutardja 18:2b7db50fec4c 413 //// velocity = 0;
vsutardja 18:2b7db50fec4c 414 //// for (int i = 0; i < 10; i++) {
vsutardja 18:2b7db50fec4c 415 //// velocity = velocity + prev_vels[i];
vsutardja 18:2b7db50fec4c 416 //// velocity = velocity / 10;
vsutardja 18:2b7db50fec4c 417 //// }
vsutardja 18:2b7db50fec4c 418 //// }
vsutardja 18:2b7db50fec4c 419 // velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
vsutardja 18:2b7db50fec4c 420 // if (velocity < 0) {
vsutardja 18:2b7db50fec4c 421 // velocity = 0;
vsutardja 18:2b7db50fec4c 422 // }
vsutardja 18:2b7db50fec4c 423 //// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
vsutardja 18:2b7db50fec4c 424 // t.reset();
vsutardja 18:2b7db50fec4c 425 // tele_velocity = velocity;
vsutardja 18:2b7db50fec4c 426 // prev_pulses = curr_pulses;
vsutardja 18:2b7db50fec4c 427 // motor_ctrl.setProcessValue(velocity);
vsutardja 18:2b7db50fec4c 428 // motor_duty = motor_ctrl.compute();
vsutardja 18:2b7db50fec4c 429 // motor = 1.0 - motor_duty;
vsutardja 18:2b7db50fec4c 430 // tele_pwm = motor_duty;
vsutardja 18:2b7db50fec4c 431 // } else {
vsutardja 18:2b7db50fec4c 432 // motor = 1.0;
vsutardja 18:2b7db50fec4c 433 // }
vsutardja 18:2b7db50fec4c 434 // // t_vel = t.read_us();
vsutardja 18:2b7db50fec4c 435 // ctrl_flag = false;
vsutardja 14:a0614f48e6ef 436 }
vsutardja 14:a0614f48e6ef 437
vsutardja 18:2b7db50fec4c 438 void set_control_flag() {
vsutardja 14:a0614f48e6ef 439 ctrl_flag = true;
vsutardja 0:fcf070a88ba0 440 }
vsutardja 0:fcf070a88ba0 441
vsutardja 0:fcf070a88ba0 442 // ====
vsutardja 0:fcf070a88ba0 443 // Main
vsutardja 0:fcf070a88ba0 444 // ====
vsutardja 0:fcf070a88ba0 445 int main() {
vsutardja 11:4348bba086a4 446 t.start();
vsutardja 12:54e7d8ff3a74 447 t_tele.start();
vsutardja 12:54e7d8ff3a74 448 tele_center.set_limits(0, 128);
vsutardja 12:54e7d8ff3a74 449 tele_pwm.set_limits(0.0, 1.0);
vsutardja 2:a8adff46eaca 450
vsutardja 18:2b7db50fec4c 451 for (int i = 0; i < 10; i++) {
vsutardja 18:2b7db50fec4c 452 prev_vels[i] = -1;
vsutardja 18:2b7db50fec4c 453 }
vsutardja 18:2b7db50fec4c 454
vsutardja 0:fcf070a88ba0 455 // Initialize motor
vsutardja 12:54e7d8ff3a74 456 motor.period_us(motor_T);
vsutardja 12:54e7d8ff3a74 457 motor = 1.0 - motor_duty;
vsutardja 0:fcf070a88ba0 458
vsutardja 0:fcf070a88ba0 459 // Initialize motor controller
vsutardja 0:fcf070a88ba0 460 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 18:2b7db50fec4c 461 motor_ctrl.setOutputLimits(0.0, 0.75);
vsutardja 4:947c3634b649 462 motor_ctrl.setSetPoint(ref_v);
vsutardja 0:fcf070a88ba0 463 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 464 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 465
vsutardja 12:54e7d8ff3a74 466 // Initialize servo
vsutardja 12:54e7d8ff3a74 467 servo.calibrate(0.001, 45.0);
vsutardja 12:54e7d8ff3a74 468 servo = angle / 180.0;
vsutardja 0:fcf070a88ba0 469
vsutardja 18:2b7db50fec4c 470 servo_ctrl.setInputLimits(10, 107);
vsutardja 16:3ab3c4670f4f 471 servo_ctrl.setOutputLimits(-25, 25);
vsutardja 16:3ab3c4670f4f 472 servo_ctrl.setSetPoint(63.5);
vsutardja 18:2b7db50fec4c 473 servo_ctrl.setBias(0.0);
vsutardja 16:3ab3c4670f4f 474 servo_ctrl.setMode(1);
vsutardja 16:3ab3c4670f4f 475
vsutardja 0:fcf070a88ba0 476 // Initialize communications thread
vsutardja 18:2b7db50fec4c 477 Thread communication_thread(communication);
vsutardja 18:2b7db50fec4c 478
vsutardja 18:2b7db50fec4c 479 control_interrupt.attach(control, interrupt_T);
vsutardja 18:2b7db50fec4c 480 // control_interrupt.attach(set_control_flag, interrupt_T);
vsutardja 12:54e7d8ff3a74 481
vsutardja 14:a0614f48e6ef 482 while (true) {
vsutardja 18:2b7db50fec4c 483 // if (ctrl_flag) {
vsutardja 18:2b7db50fec4c 484 // control();
vsutardja 18:2b7db50fec4c 485 // }
vsutardja 14:a0614f48e6ef 486 }
vsutardja 0:fcf070a88ba0 487 }