Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of FixedPWMWill by
main.cpp@16:3ab3c4670f4f, 2016-04-12 (annotated)
- Committer:
- vsutardja
- Date:
- Tue Apr 12 02:03:50 2016 +0000
- Revision:
- 16:3ab3c4670f4f
- Parent:
- 15:db95bb7c7f93
- Child:
- 17:bf6192a361ab
PID library for steering
Who changed what in which revision?
User | Revision | Line number | New 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 | 16:3ab3c4670f4f | 14 | MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX |
vsutardja | 16:3ab3c4670f4f | 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 | 12:54e7d8ff3a74 | 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 | 14:a0614f48e6ef | 40 | char comm_in[8]; // Input |
vsutardja | 16:3ab3c4670f4f | 41 | Serial bt(USBTX, USBRX); // USB connection |
vsutardja | 16:3ab3c4670f4f | 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 | 16:3ab3c4670f4f | 46 | //void Kmill(Arguments *input, Reply *output); |
vsutardja | 16:3ab3c4670f4f | 47 | //RPCFunction rpc_Kmill(&Kmill, "Kmill"); |
vsutardja | 13:d6e167698517 | 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 | 16:3ab3c4670f4f | 70 | float Kmp = 8.0; // Proportional factor |
vsutardja | 16:3ab3c4670f4f | 71 | float Kmi = 0; // Integral factor |
vsutardja | 16:3ab3c4670f4f | 72 | float Kmd = 0; // Derivative factor |
vsutardja | 16:3ab3c4670f4f | 73 | float motor_interval = 0.01; // Sampling interval |
vsutardja | 14:a0614f48e6ef | 74 | float prev_vels[10]; |
vsutardja | 14:a0614f48e6ef | 75 | float ref_v = 0.8; // Reference velocity |
vsutardja | 16:3ab3c4670f4f | 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 | 16:3ab3c4670f4f | 82 | float Ksp = 1.0; // 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 | 13:d6e167698517 | 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 | 13:d6e167698517 | 116 | |
vsutardja | 16:3ab3c4670f4f | 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 | 14:a0614f48e6ef | 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 | 14:a0614f48e6ef | 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 | 16:3ab3c4670f4f | 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 | 16:3ab3c4670f4f | 153 | bt.printf(" [2] Change Kmp\r\n"); |
vsutardja | 16:3ab3c4670f4f | 154 | bt.printf(" [3] Change Kmi\r\n"); |
vsutardja | 16:3ab3c4670f4f | 155 | bt.printf(" [4] Change Kmd\r\n"); |
vsutardja | 16:3ab3c4670f4f | 156 | bt.printf(" [5] Change Ksp\r\n"); |
vsutardja | 14:a0614f48e6ef | 157 | bt.printf(" [6] Change reference velocity\r\n"); |
vsutardja | 14:a0614f48e6ef | 158 | bt.printf(" [7] EMERGENCY STOP\r\n"); |
vsutardja | 15:db95bb7c7f93 | 159 | // bt.printf(" [8] Timing\r\n"); |
vsutardja | 15:db95bb7c7f93 | 160 | bt.printf(" [8] duty += 0.05\r\n"); |
vsutardja | 15:db95bb7c7f93 | 161 | bt.printf(" [9] duty -= 0.05\r\n"); |
vsutardja | 14:a0614f48e6ef | 162 | comm_cmd = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 163 | while (comm_cmd != 'q') { |
vsutardja | 14:a0614f48e6ef | 164 | switch(atoi(&comm_cmd)){ |
vsutardja | 14:a0614f48e6ef | 165 | case 0: |
vsutardja | 16:3ab3c4670f4f | 166 | 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 | 167 | break; |
vsutardja | 14:a0614f48e6ef | 168 | case 1: |
vsutardja | 14:a0614f48e6ef | 169 | bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int); |
vsutardja | 14:a0614f48e6ef | 170 | break; |
vsutardja | 14:a0614f48e6ef | 171 | case 2: |
vsutardja | 16:3ab3c4670f4f | 172 | bt.printf("Current: %f, New (8 digits): ", Kmp); |
vsutardja | 14:a0614f48e6ef | 173 | for (int i = 0; i < 8; i++) { |
vsutardja | 14:a0614f48e6ef | 174 | comm_in[i] = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 175 | bt.putc(comm_in[i]); |
vsutardja | 14:a0614f48e6ef | 176 | } |
vsutardja | 14:a0614f48e6ef | 177 | bt.printf("\r\n"); |
vsutardja | 16:3ab3c4670f4f | 178 | Kmp = atof(comm_in); |
vsutardja | 16:3ab3c4670f4f | 179 | motor_ctrl.setTunings(Kmp, Kmi, Kmd); |
vsutardja | 14:a0614f48e6ef | 180 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 181 | break; |
vsutardja | 14:a0614f48e6ef | 182 | case 3: |
vsutardja | 16:3ab3c4670f4f | 183 | bt.printf("Current: %f, New (8 digits): ", Kmi); |
vsutardja | 14:a0614f48e6ef | 184 | for (int i = 0; i < 8; i++) { |
vsutardja | 14:a0614f48e6ef | 185 | comm_in[i] = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 186 | bt.putc(comm_in[i]); |
vsutardja | 14:a0614f48e6ef | 187 | } |
vsutardja | 14:a0614f48e6ef | 188 | bt.printf("\r\n"); |
vsutardja | 16:3ab3c4670f4f | 189 | Kmi = atof(comm_in); |
vsutardja | 16:3ab3c4670f4f | 190 | motor_ctrl.setTunings(Kmp, Kmi, Kmd); |
vsutardja | 14:a0614f48e6ef | 191 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 192 | break; |
vsutardja | 14:a0614f48e6ef | 193 | case 4: |
vsutardja | 16:3ab3c4670f4f | 194 | bt.printf("Current: %f, New (8 digits): ", Kmd); |
vsutardja | 14:a0614f48e6ef | 195 | for (int i = 0; i < 8; i++) { |
vsutardja | 14:a0614f48e6ef | 196 | comm_in[i] = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 197 | bt.putc(comm_in[i]); |
vsutardja | 14:a0614f48e6ef | 198 | } |
vsutardja | 14:a0614f48e6ef | 199 | bt.printf("\r\n"); |
vsutardja | 16:3ab3c4670f4f | 200 | Kmd = atof(comm_in); |
vsutardja | 16:3ab3c4670f4f | 201 | motor_ctrl.setTunings(Kmp, Kmi, Kmd); |
vsutardja | 14:a0614f48e6ef | 202 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 203 | break; |
vsutardja | 14:a0614f48e6ef | 204 | case 5: |
vsutardja | 16:3ab3c4670f4f | 205 | bt.printf("Current: %f, New (8 digits): ", Ksp); |
vsutardja | 14:a0614f48e6ef | 206 | for (int i = 0; i < 8; i++) { |
vsutardja | 14:a0614f48e6ef | 207 | comm_in[i] = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 208 | bt.putc(comm_in[i]); |
vsutardja | 14:a0614f48e6ef | 209 | } |
vsutardja | 14:a0614f48e6ef | 210 | bt.printf("\r\n"); |
vsutardja | 16:3ab3c4670f4f | 211 | Ksp = atof(comm_in); |
vsutardja | 14:a0614f48e6ef | 212 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 213 | break; |
vsutardja | 14:a0614f48e6ef | 214 | case 6: |
vsutardja | 14:a0614f48e6ef | 215 | bt.printf("Current: %f, New (8 digits): ", ref_v); |
vsutardja | 14:a0614f48e6ef | 216 | for (int i = 0; i < 8; i++) { |
vsutardja | 14:a0614f48e6ef | 217 | comm_in[i] = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 218 | bt.putc(comm_in[i]); |
vsutardja | 14:a0614f48e6ef | 219 | } |
vsutardja | 14:a0614f48e6ef | 220 | bt.printf("\r\n"); |
vsutardja | 14:a0614f48e6ef | 221 | ref_v = atof(comm_in); |
vsutardja | 14:a0614f48e6ef | 222 | motor_ctrl.setSetPoint(ref_v); |
vsutardja | 14:a0614f48e6ef | 223 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 224 | break; |
vsutardja | 14:a0614f48e6ef | 225 | case 7: |
vsutardja | 15:db95bb7c7f93 | 226 | // e_stop = true; |
vsutardja | 15:db95bb7c7f93 | 227 | motor = 1.0; |
vsutardja | 14:a0614f48e6ef | 228 | bt.printf("STOPPED\r\n"); |
vsutardja | 14:a0614f48e6ef | 229 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 230 | break; |
vsutardja | 15:db95bb7c7f93 | 231 | // case 8: |
vsutardja | 15:db95bb7c7f93 | 232 | // 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 | 15:db95bb7c7f93 | 233 | // break; |
vsutardja | 14:a0614f48e6ef | 234 | case 8: |
vsutardja | 15:db95bb7c7f93 | 235 | motor_duty = motor_duty + 0.05; |
vsutardja | 15:db95bb7c7f93 | 236 | motor = 1.0 - motor_duty; |
vsutardja | 15:db95bb7c7f93 | 237 | bt.printf("%f\r\n", motor_duty); |
vsutardja | 15:db95bb7c7f93 | 238 | comm_cmd = 'q'; |
vsutardja | 15:db95bb7c7f93 | 239 | break; |
vsutardja | 15:db95bb7c7f93 | 240 | case 9: |
vsutardja | 15:db95bb7c7f93 | 241 | motor_duty = motor_duty - 0.05; |
vsutardja | 15:db95bb7c7f93 | 242 | motor = 1.0 - motor_duty; |
vsutardja | 15:db95bb7c7f93 | 243 | bt.printf("%f\r\n", motor_duty); |
vsutardja | 15:db95bb7c7f93 | 244 | comm_cmd = 'q'; |
vsutardja | 14:a0614f48e6ef | 245 | break; |
vsutardja | 14:a0614f48e6ef | 246 | } |
vsutardja | 14:a0614f48e6ef | 247 | if (bt.readable()) { |
vsutardja | 14:a0614f48e6ef | 248 | comm_cmd = bt.getc(); |
vsutardja | 14:a0614f48e6ef | 249 | } |
vsutardja | 14:a0614f48e6ef | 250 | } |
vsutardja | 14:a0614f48e6ef | 251 | } |
vsutardja | 14:a0614f48e6ef | 252 | } |
vsutardja | 14:a0614f48e6ef | 253 | |
vsutardja | 12:54e7d8ff3a74 | 254 | void control() { |
vsutardja | 12:54e7d8ff3a74 | 255 | // Image capture |
vsutardja | 14:a0614f48e6ef | 256 | // t.reset(); |
vsutardja | 12:54e7d8ff3a74 | 257 | |
vsutardja | 12:54e7d8ff3a74 | 258 | // Dummy read |
vsutardja | 13:d6e167698517 | 259 | PTD->PCOR = (0x01 << 5); |
vsutardja | 13:d6e167698517 | 260 | PTD->PSOR = (0x01); |
vsutardja | 13:d6e167698517 | 261 | PTD->PSOR = (0x01 << 5); |
vsutardja | 13:d6e167698517 | 262 | PTD->PCOR = (0x01); |
vsutardja | 11:4348bba086a4 | 263 | |
vsutardja | 12:54e7d8ff3a74 | 264 | for (int i = 0; i < 128; i++) { |
vsutardja | 13:d6e167698517 | 265 | PTD->PCOR = (0x01 << 5); |
vsutardja | 13:d6e167698517 | 266 | PTD->PSOR = (0x01 << 5); |
vsutardja | 12:54e7d8ff3a74 | 267 | } |
vsutardja | 12:54e7d8ff3a74 | 268 | |
vsutardja | 12:54e7d8ff3a74 | 269 | // Expose |
vsutardja | 12:54e7d8ff3a74 | 270 | wait_us(t_int); |
vsutardja | 12:54e7d8ff3a74 | 271 | |
vsutardja | 12:54e7d8ff3a74 | 272 | // Read camera |
vsutardja | 13:d6e167698517 | 273 | PTD->PCOR = (0x01 << 5); |
vsutardja | 13:d6e167698517 | 274 | PTD->PSOR = (0x01); |
vsutardja | 13:d6e167698517 | 275 | PTD->PSOR = (0x01 << 5); |
vsutardja | 13:d6e167698517 | 276 | PTD->PCOR = (0x01); |
vsutardja | 0:fcf070a88ba0 | 277 | |
vsutardja | 0:fcf070a88ba0 | 278 | for (int i = 0; i < 128; i++) { |
vsutardja | 13:d6e167698517 | 279 | PTD->PCOR = (0x01 << 5); |
vsutardja | 12:54e7d8ff3a74 | 280 | if (i > 9 && i < 118) { |
vsutardja | 12:54e7d8ff3a74 | 281 | img[i-10] = ao.read_u16(); |
vsutardja | 12:54e7d8ff3a74 | 282 | tele_linescan[i-10] = img[i-10]; |
vsutardja | 12:54e7d8ff3a74 | 283 | } |
vsutardja | 13:d6e167698517 | 284 | PTD->PSOR = (0x01 << 5); |
vsutardja | 0:fcf070a88ba0 | 285 | } |
vsutardja | 12:54e7d8ff3a74 | 286 | |
vsutardja | 14:a0614f48e6ef | 287 | // t_cam = t.read_us(); |
vsutardja | 0:fcf070a88ba0 | 288 | |
vsutardja | 12:54e7d8ff3a74 | 289 | // Steering control |
vsutardja | 14:a0614f48e6ef | 290 | // t.reset(); |
vsutardja | 0:fcf070a88ba0 | 291 | |
vsutardja | 12:54e7d8ff3a74 | 292 | // Detect peak edges |
vsutardja | 12:54e7d8ff3a74 | 293 | j = 0; |
vsutardja | 16:3ab3c4670f4f | 294 | for (int i = 0; i < 108 && j < 5; i++) { |
vsutardja | 12:54e7d8ff3a74 | 295 | if (img[i] > 45000) { |
vsutardja | 12:54e7d8ff3a74 | 296 | left[j] = i; |
vsutardja | 16:3ab3c4670f4f | 297 | while (img[i] > 45000 && i < 108) { |
vsutardja | 12:54e7d8ff3a74 | 298 | i = i + 1; |
vsutardja | 0:fcf070a88ba0 | 299 | } |
vsutardja | 12:54e7d8ff3a74 | 300 | right[j] = i; |
vsutardja | 12:54e7d8ff3a74 | 301 | j = j + 1; |
vsutardja | 0:fcf070a88ba0 | 302 | } |
vsutardja | 0:fcf070a88ba0 | 303 | } |
vsutardja | 0:fcf070a88ba0 | 304 | |
vsutardja | 12:54e7d8ff3a74 | 305 | // Calculate peak centers |
vsutardja | 12:54e7d8ff3a74 | 306 | for (int i = 0; i < j; i++) { |
vsutardja | 16:3ab3c4670f4f | 307 | centers[i] = (left[i] + right[i] + 20) / 2; |
vsutardja | 12:54e7d8ff3a74 | 308 | } |
vsutardja | 12:54e7d8ff3a74 | 309 | |
vsutardja | 12:54e7d8ff3a74 | 310 | // Assign scores |
vsutardja | 12:54e7d8ff3a74 | 311 | for (int i = 0; i < j; i++) { |
vsutardja | 16:3ab3c4670f4f | 312 | scores[i] = 8 / (right[i] - left[i]) + img[centers[i]] / 65536 + 5 / abs(centers[i] - prev_center); |
vsutardja | 3:c57674c348bd | 313 | } |
vsutardja | 3:c57674c348bd | 314 | |
vsutardja | 12:54e7d8ff3a74 | 315 | // Choose most likely center |
vsutardja | 12:54e7d8ff3a74 | 316 | best_score_idx = 0; |
vsutardja | 12:54e7d8ff3a74 | 317 | for (int i = 0; i < j; i++) { |
vsutardja | 12:54e7d8ff3a74 | 318 | if (scores[i] > scores[best_score_idx]) { |
vsutardja | 12:54e7d8ff3a74 | 319 | best_score_idx = i; |
vsutardja | 12:54e7d8ff3a74 | 320 | } |
vsutardja | 12:54e7d8ff3a74 | 321 | } |
vsutardja | 12:54e7d8ff3a74 | 322 | prev_center = center; |
vsutardja | 12:54e7d8ff3a74 | 323 | center = centers[best_score_idx]; |
vsutardja | 12:54e7d8ff3a74 | 324 | tele_center = center; |
vsutardja | 3:c57674c348bd | 325 | |
vsutardja | 12:54e7d8ff3a74 | 326 | // Set servo angle |
vsutardja | 16:3ab3c4670f4f | 327 | // angle = 88 + (55 - center) * Ksp; |
vsutardja | 16:3ab3c4670f4f | 328 | // if (angle > 113) { |
vsutardja | 16:3ab3c4670f4f | 329 | // angle = 113; |
vsutardja | 16:3ab3c4670f4f | 330 | // } |
vsutardja | 16:3ab3c4670f4f | 331 | // if (angle < 63) { |
vsutardja | 16:3ab3c4670f4f | 332 | // angle = 63; |
vsutardja | 16:3ab3c4670f4f | 333 | // } |
vsutardja | 16:3ab3c4670f4f | 334 | // servo = angle / 180; |
vsutardja | 16:3ab3c4670f4f | 335 | |
vsutardja | 16:3ab3c4670f4f | 336 | servo_ctrl.setProcessValue(center); |
vsutardja | 16:3ab3c4670f4f | 337 | angle = 88 + servo_ctrl.compute(); |
vsutardja | 12:54e7d8ff3a74 | 338 | servo = angle / 180; |
vsutardja | 12:54e7d8ff3a74 | 339 | |
vsutardja | 12:54e7d8ff3a74 | 340 | // AGC |
vsutardja | 12:54e7d8ff3a74 | 341 | max = -1; |
vsutardja | 12:54e7d8ff3a74 | 342 | for (int i = 0; i < 107; i++) { |
vsutardja | 12:54e7d8ff3a74 | 343 | if (img[i] > max) { |
vsutardja | 12:54e7d8ff3a74 | 344 | max = img[i]; |
vsutardja | 12:54e7d8ff3a74 | 345 | } |
vsutardja | 12:54e7d8ff3a74 | 346 | } |
vsutardja | 11:4348bba086a4 | 347 | if (max > 60000) { |
vsutardja | 11:4348bba086a4 | 348 | t_int = t_int - 0.1 * (max - 60000); |
vsutardja | 11:4348bba086a4 | 349 | } |
vsutardja | 11:4348bba086a4 | 350 | if (max < 50000) { |
vsutardja | 11:4348bba086a4 | 351 | t_int = t_int + 0.1 * (50000 - max); |
vsutardja | 11:4348bba086a4 | 352 | } |
vsutardja | 12:54e7d8ff3a74 | 353 | if (t_int < T_INT_MIN) { |
vsutardja | 12:54e7d8ff3a74 | 354 | t_int = T_INT_MIN; |
vsutardja | 12:54e7d8ff3a74 | 355 | } |
vsutardja | 12:54e7d8ff3a74 | 356 | if (t_int > T_INT_MAX) { |
vsutardja | 12:54e7d8ff3a74 | 357 | t_int = T_INT_MAX; |
vsutardja | 11:4348bba086a4 | 358 | } |
vsutardja | 12:54e7d8ff3a74 | 359 | tele_exposure = t_int; |
vsutardja | 11:4348bba086a4 | 360 | |
vsutardja | 14:a0614f48e6ef | 361 | // t_steer = t.read_us(); |
vsutardja | 11:4348bba086a4 | 362 | |
vsutardja | 15:db95bb7c7f93 | 363 | // // Velocity control |
vsutardja | 15:db95bb7c7f93 | 364 | // // t.reset(); |
vsutardja | 15:db95bb7c7f93 | 365 | // if (!e_stop) { |
vsutardja | 15:db95bb7c7f93 | 366 | // curr_pulses = qei.getPulses(); |
vsutardja | 15:db95bb7c7f93 | 367 | // //for (int i = 0; i < 9; i++) { |
vsutardja | 15:db95bb7c7f93 | 368 | //// prev_vels[i] = prev_vels[i+1]; |
vsutardja | 15:db95bb7c7f93 | 369 | //// } |
vsutardja | 15:db95bb7c7f93 | 370 | //// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c; |
vsutardja | 15:db95bb7c7f93 | 371 | //// t.reset(); |
vsutardja | 15:db95bb7c7f93 | 372 | //// if (prev_vels[9] < 0) { |
vsutardja | 15:db95bb7c7f93 | 373 | //// prev_vels[9] = 0; |
vsutardja | 15:db95bb7c7f93 | 374 | //// } |
vsutardja | 15:db95bb7c7f93 | 375 | //// if (prev_vels[0] < 0) { |
vsutardja | 15:db95bb7c7f93 | 376 | //// velocity = prev_vels[9]; |
vsutardja | 15:db95bb7c7f93 | 377 | //// } else { |
vsutardja | 15:db95bb7c7f93 | 378 | //// velocity = 0; |
vsutardja | 15:db95bb7c7f93 | 379 | //// for (int i = 0; i < 10; i++) { |
vsutardja | 15:db95bb7c7f93 | 380 | //// velocity = velocity + prev_vels[i]; |
vsutardja | 15:db95bb7c7f93 | 381 | //// velocity = velocity / 10; |
vsutardja | 15:db95bb7c7f93 | 382 | //// } |
vsutardja | 15:db95bb7c7f93 | 383 | //// } |
vsutardja | 15:db95bb7c7f93 | 384 | // velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c; |
vsutardja | 15:db95bb7c7f93 | 385 | // if (velocity < 0) { |
vsutardja | 14:a0614f48e6ef | 386 | // velocity = 0; |
vsutardja | 14:a0614f48e6ef | 387 | // } |
vsutardja | 15:db95bb7c7f93 | 388 | //// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c; |
vsutardja | 15:db95bb7c7f93 | 389 | // t.reset(); |
vsutardja | 15:db95bb7c7f93 | 390 | // tele_velocity = velocity; |
vsutardja | 15:db95bb7c7f93 | 391 | // prev_pulses = curr_pulses; |
vsutardja | 15:db95bb7c7f93 | 392 | // motor_ctrl.setProcessValue(velocity); |
vsutardja | 15:db95bb7c7f93 | 393 | // motor_duty = motor_ctrl.compute(); |
vsutardja | 15:db95bb7c7f93 | 394 | // motor = 1.0 - motor_duty; |
vsutardja | 15:db95bb7c7f93 | 395 | // tele_pwm = motor_duty; |
vsutardja | 15:db95bb7c7f93 | 396 | // } else { |
vsutardja | 15:db95bb7c7f93 | 397 | // motor = 1.0; |
vsutardja | 15:db95bb7c7f93 | 398 | // } |
vsutardja | 15:db95bb7c7f93 | 399 | // // t_vel = t.read_us(); |
vsutardja | 15:db95bb7c7f93 | 400 | // ctrl_flag = false; |
vsutardja | 14:a0614f48e6ef | 401 | } |
vsutardja | 14:a0614f48e6ef | 402 | |
vsutardja | 14:a0614f48e6ef | 403 | void set_control_flag() { |
vsutardja | 14:a0614f48e6ef | 404 | ctrl_flag = true; |
vsutardja | 0:fcf070a88ba0 | 405 | } |
vsutardja | 0:fcf070a88ba0 | 406 | |
vsutardja | 0:fcf070a88ba0 | 407 | // ==== |
vsutardja | 0:fcf070a88ba0 | 408 | // Main |
vsutardja | 0:fcf070a88ba0 | 409 | // ==== |
vsutardja | 0:fcf070a88ba0 | 410 | int main() { |
vsutardja | 11:4348bba086a4 | 411 | t.start(); |
vsutardja | 12:54e7d8ff3a74 | 412 | t_tele.start(); |
vsutardja | 12:54e7d8ff3a74 | 413 | tele_center.set_limits(0, 128); |
vsutardja | 12:54e7d8ff3a74 | 414 | tele_pwm.set_limits(0.0, 1.0); |
vsutardja | 2:a8adff46eaca | 415 | |
vsutardja | 14:a0614f48e6ef | 416 | for (int i = 0; i < 10; i++) { |
vsutardja | 14:a0614f48e6ef | 417 | prev_vels[i] = -1; |
vsutardja | 14:a0614f48e6ef | 418 | } |
vsutardja | 14:a0614f48e6ef | 419 | |
vsutardja | 0:fcf070a88ba0 | 420 | // Initialize motor |
vsutardja | 12:54e7d8ff3a74 | 421 | motor.period_us(motor_T); |
vsutardja | 12:54e7d8ff3a74 | 422 | motor = 1.0 - motor_duty; |
vsutardja | 0:fcf070a88ba0 | 423 | |
vsutardja | 0:fcf070a88ba0 | 424 | // Initialize motor controller |
vsutardja | 0:fcf070a88ba0 | 425 | motor_ctrl.setInputLimits(0.0, 10.0); |
vsutardja | 14:a0614f48e6ef | 426 | motor_ctrl.setOutputLimits(0.0, 0.75); |
vsutardja | 4:947c3634b649 | 427 | motor_ctrl.setSetPoint(ref_v); |
vsutardja | 0:fcf070a88ba0 | 428 | motor_ctrl.setBias(0.0); |
vsutardja | 0:fcf070a88ba0 | 429 | motor_ctrl.setMode(1); |
vsutardja | 0:fcf070a88ba0 | 430 | |
vsutardja | 12:54e7d8ff3a74 | 431 | // Initialize servo |
vsutardja | 12:54e7d8ff3a74 | 432 | servo.calibrate(0.001, 45.0); |
vsutardja | 12:54e7d8ff3a74 | 433 | servo = angle / 180.0; |
vsutardja | 0:fcf070a88ba0 | 434 | |
vsutardja | 16:3ab3c4670f4f | 435 | // Initialize servo controller |
vsutardja | 16:3ab3c4670f4f | 436 | servo_ctrl.setInputLimits(10, 117); |
vsutardja | 16:3ab3c4670f4f | 437 | servo_ctrl.setOutputLimits(-25, 25); |
vsutardja | 16:3ab3c4670f4f | 438 | servo_ctrl.setSetPoint(63.5); |
vsutardja | 16:3ab3c4670f4f | 439 | servo_ctrl.setBias(0.0); |
vsutardja | 16:3ab3c4670f4f | 440 | servo_ctrl.setMode(1); |
vsutardja | 16:3ab3c4670f4f | 441 | |
vsutardja | 0:fcf070a88ba0 | 442 | // Initialize communications thread |
vsutardja | 12:54e7d8ff3a74 | 443 | Thread communication_thread(communication); |
vsutardja | 14:a0614f48e6ef | 444 | |
vsutardja | 15:db95bb7c7f93 | 445 | control_interrupt.attach(control, interrupt_T); |
vsutardja | 15:db95bb7c7f93 | 446 | // control_interrupt.attach(set_control_flag, interrupt_T); |
vsutardja | 12:54e7d8ff3a74 | 447 | |
vsutardja | 14:a0614f48e6ef | 448 | while (true) { |
vsutardja | 15:db95bb7c7f93 | 449 | // if (ctrl_flag) { |
vsutardja | 15:db95bb7c7f93 | 450 | // control(); |
vsutardja | 15:db95bb7c7f93 | 451 | // } |
vsutardja | 14:a0614f48e6ef | 452 | } |
vsutardja | 0:fcf070a88ba0 | 453 | } |