One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
main.cpp@8:71c39d2f80e2, 2016-04-01 (annotated)
- Committer:
- vsutardja
- Date:
- Fri Apr 01 21:41:41 2016 +0000
- Revision:
- 8:71c39d2f80e2
- Parent:
- 7:8229baaf1fbf
- Child:
- 9:10fcf3d0ec2d
Velocity and center telemetry
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsutardja | 0:fcf070a88ba0 | 1 | #include "mbed.h" |
vsutardja | 0:fcf070a88ba0 | 2 | #include "rtos.h" |
vsutardja | 0:fcf070a88ba0 | 3 | #include "Servo.h" |
vsutardja | 0:fcf070a88ba0 | 4 | #include "FastAnalogIn.h" |
vsutardja | 0:fcf070a88ba0 | 5 | #include "PID.h" |
vsutardja | 0:fcf070a88ba0 | 6 | #include "QEI.h" |
vsutardja | 0:fcf070a88ba0 | 7 | #include "telemetry.h" |
vsutardja | 0:fcf070a88ba0 | 8 | |
vsutardja | 0:fcf070a88ba0 | 9 | // ========= |
vsutardja | 0:fcf070a88ba0 | 10 | // Telemetry |
vsutardja | 0:fcf070a88ba0 | 11 | // ========= |
vsutardja | 7:8229baaf1fbf | 12 | MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX |
vsutardja | 7:8229baaf1fbf | 13 | telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer |
vsutardja | 7:8229baaf1fbf | 14 | telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry |
vsutardja | 7:8229baaf1fbf | 15 | |
vsutardja | 7:8229baaf1fbf | 16 | int dec = 0; |
vsutardja | 7:8229baaf1fbf | 17 | Timer t; |
vsutardja | 7:8229baaf1fbf | 18 | telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); |
vsutardja | 7:8229baaf1fbf | 19 | telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); |
vsutardja | 8:71c39d2f80e2 | 20 | telemetry::Numeric<float> tele_vel(telemetry_obj, "t_vel", "Velocity", "m/s", 0); |
vsutardja | 8:71c39d2f80e2 | 21 | telemetry::Numeric<uint32_t> tele_center(telemetry_obj, "t_center", "Center", "", 0); |
vsutardja | 0:fcf070a88ba0 | 22 | |
vsutardja | 0:fcf070a88ba0 | 23 | |
vsutardja | 0:fcf070a88ba0 | 24 | // ============= |
vsutardja | 0:fcf070a88ba0 | 25 | // Communication |
vsutardja | 0:fcf070a88ba0 | 26 | // ============= |
vsutardja | 0:fcf070a88ba0 | 27 | Serial pc(USBTX, USBRX); // USB connection |
vsutardja | 7:8229baaf1fbf | 28 | //Serial bt(PTC4, PTC3); // BlueSMiRF connection |
vsutardja | 0:fcf070a88ba0 | 29 | char cmd; // Command |
vsutardja | 0:fcf070a88ba0 | 30 | char ch; |
vsutardja | 4:947c3634b649 | 31 | char in[5]; |
vsutardja | 0:fcf070a88ba0 | 32 | |
vsutardja | 0:fcf070a88ba0 | 33 | void communication(void const *args); // Communications |
vsutardja | 0:fcf070a88ba0 | 34 | |
vsutardja | 0:fcf070a88ba0 | 35 | // ===== |
vsutardja | 0:fcf070a88ba0 | 36 | // Motor |
vsutardja | 0:fcf070a88ba0 | 37 | // ===== |
vsutardja | 0:fcf070a88ba0 | 38 | PwmOut motor(PTA4); // Enable pin (PWM) |
vsutardja | 0:fcf070a88ba0 | 39 | int T = 25000; // Frequency |
vsutardja | 0:fcf070a88ba0 | 40 | float d = 0.0; // Duty cycle |
vsutardja | 0:fcf070a88ba0 | 41 | |
vsutardja | 0:fcf070a88ba0 | 42 | // ======= |
vsutardja | 0:fcf070a88ba0 | 43 | // Encoder |
vsutardja | 0:fcf070a88ba0 | 44 | // ======= |
vsutardja | 2:a8adff46eaca | 45 | const int MVG_AVG = 4; |
vsutardja | 0:fcf070a88ba0 | 46 | int ppr = 389; // Pulses per revolution |
vsutardja | 0:fcf070a88ba0 | 47 | QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder |
vsutardja | 0:fcf070a88ba0 | 48 | float c = 0.20106; // Wheel circumference |
vsutardja | 0:fcf070a88ba0 | 49 | int prev_pulses = 0; // Previous pulse count |
vsutardja | 0:fcf070a88ba0 | 50 | int curr_pulses = 0; // Current pulse count |
vsutardja | 0:fcf070a88ba0 | 51 | float velocity = 0; // Velocity |
vsutardja | 4:947c3634b649 | 52 | float vel = 0; |
vsutardja | 2:a8adff46eaca | 53 | float v_prev[MVG_AVG] = {0}; |
vsutardja | 0:fcf070a88ba0 | 54 | |
vsutardja | 0:fcf070a88ba0 | 55 | // ======== |
vsutardja | 0:fcf070a88ba0 | 56 | // Velocity |
vsutardja | 0:fcf070a88ba0 | 57 | // ======== |
vsutardja | 8:71c39d2f80e2 | 58 | float Kp = 3.0; // Proportional factor |
vsutardja | 8:71c39d2f80e2 | 59 | float Ki = 1; // Integral factor |
vsutardja | 8:71c39d2f80e2 | 60 | float Kd = 0.0001; // Derivative factor |
vsutardja | 0:fcf070a88ba0 | 61 | float interval = 0.01; // Sampling interval |
vsutardja | 4:947c3634b649 | 62 | float ref_v = 1.0; |
vsutardja | 0:fcf070a88ba0 | 63 | PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller |
vsutardja | 0:fcf070a88ba0 | 64 | |
vsutardja | 0:fcf070a88ba0 | 65 | // ===== |
vsutardja | 0:fcf070a88ba0 | 66 | // Servo |
vsutardja | 0:fcf070a88ba0 | 67 | // ===== |
vsutardja | 0:fcf070a88ba0 | 68 | Servo servo(PTA12); // Enable pin (PWM) |
vsutardja | 0:fcf070a88ba0 | 69 | float a = 88; // Angle |
vsutardja | 4:947c3634b649 | 70 | float Ks = 0.7; |
vsutardja | 0:fcf070a88ba0 | 71 | |
vsutardja | 0:fcf070a88ba0 | 72 | // ====== |
vsutardja | 0:fcf070a88ba0 | 73 | // Camera |
vsutardja | 0:fcf070a88ba0 | 74 | // ====== |
vsutardja | 0:fcf070a88ba0 | 75 | DigitalOut clk(PTD5); // Clock pin |
vsutardja | 0:fcf070a88ba0 | 76 | DigitalOut si(PTD0); // SI pin |
vsutardja | 0:fcf070a88ba0 | 77 | FastAnalogIn ao(PTC2); // AO pin |
vsutardja | 0:fcf070a88ba0 | 78 | Timeout camera_read; // Camera read timeout |
vsutardja | 0:fcf070a88ba0 | 79 | int t_int = 15000; // Exposure time |
vsutardja | 0:fcf070a88ba0 | 80 | int img[128]; // Image data |
vsutardja | 0:fcf070a88ba0 | 81 | |
vsutardja | 0:fcf070a88ba0 | 82 | void readCamera(); // Read data from camera |
vsutardja | 0:fcf070a88ba0 | 83 | |
vsutardja | 0:fcf070a88ba0 | 84 | // ================ |
vsutardja | 0:fcf070a88ba0 | 85 | // Image processing |
vsutardja | 0:fcf070a88ba0 | 86 | // ================ |
vsutardja | 0:fcf070a88ba0 | 87 | int max = -1; |
vsutardja | 3:c57674c348bd | 88 | float lum_bg = 0; |
vsutardja | 3:c57674c348bd | 89 | float contrast = 0; |
vsutardja | 0:fcf070a88ba0 | 90 | int argmax = 0; |
vsutardja | 0:fcf070a88ba0 | 91 | int argmin = 0; |
vsutardja | 0:fcf070a88ba0 | 92 | int temp[128]; |
vsutardja | 4:947c3634b649 | 93 | int center = 64; |
vsutardja | 0:fcf070a88ba0 | 94 | |
vsutardja | 0:fcf070a88ba0 | 95 | void track(); // Line-tracking steering |
vsutardja | 0:fcf070a88ba0 | 96 | |
vsutardja | 0:fcf070a88ba0 | 97 | // ================ |
vsutardja | 0:fcf070a88ba0 | 98 | // Functions |
vsutardja | 0:fcf070a88ba0 | 99 | // ================ |
vsutardja | 0:fcf070a88ba0 | 100 | |
vsutardja | 0:fcf070a88ba0 | 101 | // Communications |
vsutardja | 7:8229baaf1fbf | 102 | //void communication(void const *args) { |
vsutardja | 7:8229baaf1fbf | 103 | // while (true) { |
vsutardja | 7:8229baaf1fbf | 104 | // bt.printf("\r\nPress q to return to this prompt.\r\n"); |
vsutardja | 7:8229baaf1fbf | 105 | // bt.printf("Available diagnostics:\r\n"); |
vsutardja | 7:8229baaf1fbf | 106 | // bt.printf(" [0] Velocity\r\n"); |
vsutardja | 7:8229baaf1fbf | 107 | // bt.printf(" [1] Steering\r\n"); |
vsutardja | 7:8229baaf1fbf | 108 | // bt.printf(" [2] Change Kp\r\n"); |
vsutardja | 7:8229baaf1fbf | 109 | // bt.printf(" [3] Change Ki\r\n"); |
vsutardja | 7:8229baaf1fbf | 110 | // bt.printf(" [4] Change Kd\r\n"); |
vsutardja | 7:8229baaf1fbf | 111 | // bt.printf(" [5] Change Ks\r\n"); |
vsutardja | 7:8229baaf1fbf | 112 | // bt.printf(" [6] Change reference velocity\r\n"); |
vsutardja | 7:8229baaf1fbf | 113 | // cmd = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 114 | // while (cmd != 'q') { |
vsutardja | 7:8229baaf1fbf | 115 | // switch(atoi(&cmd)){ |
vsutardja | 7:8229baaf1fbf | 116 | // case 0: |
vsutardja | 7:8229baaf1fbf | 117 | // 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); |
vsutardja | 7:8229baaf1fbf | 118 | // break; |
vsutardja | 7:8229baaf1fbf | 119 | // case 1: |
vsutardja | 7:8229baaf1fbf | 120 | // bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); |
vsutardja | 7:8229baaf1fbf | 121 | // break; |
vsutardja | 7:8229baaf1fbf | 122 | // case 2: |
vsutardja | 7:8229baaf1fbf | 123 | // bt.printf("Current: %f, New (5 digits): ", Kp); |
vsutardja | 7:8229baaf1fbf | 124 | // for (int i = 0; i < 5; i++) { |
vsutardja | 7:8229baaf1fbf | 125 | // in[i] = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 126 | // bt.putc(in[i]); |
vsutardja | 7:8229baaf1fbf | 127 | // } |
vsutardja | 7:8229baaf1fbf | 128 | // bt.printf("\r\n"); |
vsutardja | 7:8229baaf1fbf | 129 | // Kp = atof(in); |
vsutardja | 7:8229baaf1fbf | 130 | // motor_ctrl.setTunings(Kp, Ki, Kd); |
vsutardja | 7:8229baaf1fbf | 131 | // cmd = 'q'; |
vsutardja | 7:8229baaf1fbf | 132 | // break; |
vsutardja | 7:8229baaf1fbf | 133 | // case 3: |
vsutardja | 7:8229baaf1fbf | 134 | // bt.printf("Current: %f, New (5 digits): ", Ki); |
vsutardja | 7:8229baaf1fbf | 135 | // for (int i = 0; i < 5; i++) { |
vsutardja | 7:8229baaf1fbf | 136 | // in[i] = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 137 | // bt.putc(in[i]); |
vsutardja | 7:8229baaf1fbf | 138 | // } |
vsutardja | 7:8229baaf1fbf | 139 | // bt.printf("\r\n"); |
vsutardja | 7:8229baaf1fbf | 140 | // Ki = atof(in); |
vsutardja | 7:8229baaf1fbf | 141 | // motor_ctrl.setTunings(Kp, Ki, Kd); |
vsutardja | 7:8229baaf1fbf | 142 | // cmd = 'q'; |
vsutardja | 7:8229baaf1fbf | 143 | // break; |
vsutardja | 7:8229baaf1fbf | 144 | // case 4: |
vsutardja | 7:8229baaf1fbf | 145 | // bt.printf("Current: %f, New (5 digits): ", Kd); |
vsutardja | 7:8229baaf1fbf | 146 | // for (int i = 0; i < 5; i++) { |
vsutardja | 7:8229baaf1fbf | 147 | // in[i] = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 148 | // bt.putc(in[i]); |
vsutardja | 7:8229baaf1fbf | 149 | // } |
vsutardja | 7:8229baaf1fbf | 150 | // bt.printf("\r\n"); |
vsutardja | 7:8229baaf1fbf | 151 | // Kd = atof(in); |
vsutardja | 7:8229baaf1fbf | 152 | // motor_ctrl.setTunings(Kp, Ki, Kd); |
vsutardja | 7:8229baaf1fbf | 153 | // cmd = 'q'; |
vsutardja | 7:8229baaf1fbf | 154 | // break; |
vsutardja | 7:8229baaf1fbf | 155 | // case 5: |
vsutardja | 7:8229baaf1fbf | 156 | // bt.printf("Current: %f, New (5 digits): ", Ks); |
vsutardja | 7:8229baaf1fbf | 157 | // for (int i = 0; i < 5; i++) { |
vsutardja | 7:8229baaf1fbf | 158 | // in[i] = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 159 | // bt.putc(in[i]); |
vsutardja | 7:8229baaf1fbf | 160 | // } |
vsutardja | 7:8229baaf1fbf | 161 | // bt.printf("\r\n"); |
vsutardja | 7:8229baaf1fbf | 162 | // Ks = atof(in); |
vsutardja | 7:8229baaf1fbf | 163 | // cmd = 'q'; |
vsutardja | 7:8229baaf1fbf | 164 | // break; |
vsutardja | 7:8229baaf1fbf | 165 | // case 6: |
vsutardja | 7:8229baaf1fbf | 166 | // bt.printf("Current: %f, New (5 digits): ", ref_v); |
vsutardja | 7:8229baaf1fbf | 167 | // for (int i = 0; i < 5; i++) { |
vsutardja | 7:8229baaf1fbf | 168 | // in[i] = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 169 | // bt.putc(in[i]); |
vsutardja | 7:8229baaf1fbf | 170 | // } |
vsutardja | 7:8229baaf1fbf | 171 | // bt.printf("\r\n"); |
vsutardja | 7:8229baaf1fbf | 172 | // ref_v = atof(in); |
vsutardja | 7:8229baaf1fbf | 173 | // motor_ctrl.setSetPoint(ref_v); |
vsutardja | 7:8229baaf1fbf | 174 | // cmd = 'q'; |
vsutardja | 7:8229baaf1fbf | 175 | // break; |
vsutardja | 7:8229baaf1fbf | 176 | // } |
vsutardja | 7:8229baaf1fbf | 177 | // if (bt.readable()) { |
vsutardja | 7:8229baaf1fbf | 178 | // cmd = bt.getc(); |
vsutardja | 7:8229baaf1fbf | 179 | // } |
vsutardja | 7:8229baaf1fbf | 180 | // } |
vsutardja | 7:8229baaf1fbf | 181 | // } |
vsutardja | 7:8229baaf1fbf | 182 | //} |
vsutardja | 0:fcf070a88ba0 | 183 | |
vsutardja | 0:fcf070a88ba0 | 184 | // Read data from camera |
vsutardja | 0:fcf070a88ba0 | 185 | void read_camera() { |
vsutardja | 0:fcf070a88ba0 | 186 | // Start data transfer |
vsutardja | 0:fcf070a88ba0 | 187 | si = 1; |
vsutardja | 0:fcf070a88ba0 | 188 | wait_us(1); |
vsutardja | 0:fcf070a88ba0 | 189 | clk = 1; |
vsutardja | 0:fcf070a88ba0 | 190 | wait_us(1); |
vsutardja | 0:fcf070a88ba0 | 191 | si = 0; |
vsutardja | 0:fcf070a88ba0 | 192 | wait_us(1); |
vsutardja | 0:fcf070a88ba0 | 193 | |
vsutardja | 0:fcf070a88ba0 | 194 | // Read camera data |
vsutardja | 0:fcf070a88ba0 | 195 | for (int i = 0; i < 128; i++) { |
vsutardja | 0:fcf070a88ba0 | 196 | clk = 0; |
vsutardja | 0:fcf070a88ba0 | 197 | img[i] = ao.read_u16(); |
vsutardja | 7:8229baaf1fbf | 198 | tele_linescan[i] = img[i]; |
vsutardja | 0:fcf070a88ba0 | 199 | clk = 1; |
vsutardja | 0:fcf070a88ba0 | 200 | wait_us(1); |
vsutardja | 0:fcf070a88ba0 | 201 | } |
vsutardja | 0:fcf070a88ba0 | 202 | clk = 0; |
vsutardja | 0:fcf070a88ba0 | 203 | |
vsutardja | 0:fcf070a88ba0 | 204 | // Update servo angle |
vsutardja | 0:fcf070a88ba0 | 205 | track(); |
vsutardja | 0:fcf070a88ba0 | 206 | |
vsutardja | 0:fcf070a88ba0 | 207 | // Set next frame exposure time |
vsutardja | 3:c57674c348bd | 208 | // camera_read.attach_us(&read_camera, t_int); |
vsutardja | 0:fcf070a88ba0 | 209 | } |
vsutardja | 0:fcf070a88ba0 | 210 | |
vsutardja | 0:fcf070a88ba0 | 211 | // Find line center from image |
vsutardja | 0:fcf070a88ba0 | 212 | // Take two-point moving average to smooth the data |
vsutardja | 0:fcf070a88ba0 | 213 | // Find indices where max and min of smoothed data are attained |
vsutardja | 0:fcf070a88ba0 | 214 | // Calculate and return midpoint of argmax and argmin |
vsutardja | 0:fcf070a88ba0 | 215 | void track() { |
vsutardja | 0:fcf070a88ba0 | 216 | max = -1; |
vsutardja | 3:c57674c348bd | 217 | lum_bg = 0; |
vsutardja | 0:fcf070a88ba0 | 218 | argmax = 0; |
vsutardja | 0:fcf070a88ba0 | 219 | argmin = 0; |
vsutardja | 0:fcf070a88ba0 | 220 | for (int i = 0; i < 107; i++) { |
vsutardja | 0:fcf070a88ba0 | 221 | if (img[i+11] > max) { |
vsutardja | 0:fcf070a88ba0 | 222 | max = img[i+11]; |
vsutardja | 0:fcf070a88ba0 | 223 | } |
vsutardja | 0:fcf070a88ba0 | 224 | if (i == 126) { |
vsutardja | 0:fcf070a88ba0 | 225 | temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1]; |
vsutardja | 0:fcf070a88ba0 | 226 | if (temp[i-1] > temp[argmax]) { |
vsutardja | 0:fcf070a88ba0 | 227 | argmax = i - 1; |
vsutardja | 0:fcf070a88ba0 | 228 | } |
vsutardja | 0:fcf070a88ba0 | 229 | if (temp[i-1] < temp[argmin]) { |
vsutardja | 0:fcf070a88ba0 | 230 | argmin = i - 1; |
vsutardja | 0:fcf070a88ba0 | 231 | } |
vsutardja | 0:fcf070a88ba0 | 232 | } else { |
vsutardja | 0:fcf070a88ba0 | 233 | temp[i] = (img[i+11] + img[i+1+11]) / 2; |
vsutardja | 0:fcf070a88ba0 | 234 | if (i > 0) { |
vsutardja | 0:fcf070a88ba0 | 235 | temp[i-1] = temp[i] - temp[i-1]; |
vsutardja | 0:fcf070a88ba0 | 236 | if (temp[i-1] > temp[argmax]) { |
vsutardja | 0:fcf070a88ba0 | 237 | argmax = i - 1; |
vsutardja | 0:fcf070a88ba0 | 238 | } |
vsutardja | 0:fcf070a88ba0 | 239 | if (temp[i-1] < temp[argmin]) { |
vsutardja | 0:fcf070a88ba0 | 240 | argmin = i - 1; |
vsutardja | 0:fcf070a88ba0 | 241 | } |
vsutardja | 0:fcf070a88ba0 | 242 | } |
vsutardja | 0:fcf070a88ba0 | 243 | } |
vsutardja | 0:fcf070a88ba0 | 244 | } |
vsutardja | 0:fcf070a88ba0 | 245 | |
vsutardja | 4:947c3634b649 | 246 | for (int i = 0; i < 10; i++) { |
vsutardja | 4:947c3634b649 | 247 | lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0; |
vsutardja | 3:c57674c348bd | 248 | } |
vsutardja | 3:c57674c348bd | 249 | |
vsutardja | 3:c57674c348bd | 250 | contrast = (max - lum_bg) / lum_bg; |
vsutardja | 3:c57674c348bd | 251 | |
vsutardja | 4:947c3634b649 | 252 | // if (contrast < 1.5) { |
vsutardja | 3:c57674c348bd | 253 | // Underexposed |
vsutardja | 4:947c3634b649 | 254 | if (max < 60000) { |
vsutardja | 4:947c3634b649 | 255 | t_int = t_int + 0.15 * (60000 - max); |
vsutardja | 3:c57674c348bd | 256 | } |
vsutardja | 3:c57674c348bd | 257 | // Overexposed |
vsutardja | 3:c57674c348bd | 258 | if (lum_bg > 25000) { |
vsutardja | 4:947c3634b649 | 259 | t_int = t_int - 0.15 * (lum_bg - 25000); |
vsutardja | 3:c57674c348bd | 260 | } |
vsutardja | 4:947c3634b649 | 261 | // } |
vsutardja | 3:c57674c348bd | 262 | |
vsutardja | 0:fcf070a88ba0 | 263 | if (max > 43253) { |
vsutardja | 0:fcf070a88ba0 | 264 | center = (argmax + argmin + 2 + 11) / 2; |
vsutardja | 8:71c39d2f80e2 | 265 | tele_center = center; |
vsutardja | 4:947c3634b649 | 266 | a = 88 + (64 - center) * Ks; |
vsutardja | 0:fcf070a88ba0 | 267 | servo = a / 180; |
vsutardja | 0:fcf070a88ba0 | 268 | } |
vsutardja | 3:c57674c348bd | 269 | |
vsutardja | 3:c57674c348bd | 270 | camera_read.attach_us(&read_camera, t_int); |
vsutardja | 0:fcf070a88ba0 | 271 | } |
vsutardja | 0:fcf070a88ba0 | 272 | |
vsutardja | 0:fcf070a88ba0 | 273 | // ==== |
vsutardja | 0:fcf070a88ba0 | 274 | // Main |
vsutardja | 0:fcf070a88ba0 | 275 | // ==== |
vsutardja | 0:fcf070a88ba0 | 276 | int main() { |
vsutardja | 7:8229baaf1fbf | 277 | t.start(); |
vsutardja | 2:a8adff46eaca | 278 | |
vsutardja | 0:fcf070a88ba0 | 279 | // Initialize motor |
vsutardja | 0:fcf070a88ba0 | 280 | motor.period_us(T); |
vsutardja | 0:fcf070a88ba0 | 281 | motor = 1.0 - d; |
vsutardja | 0:fcf070a88ba0 | 282 | |
vsutardja | 0:fcf070a88ba0 | 283 | // Initialize servo |
vsutardja | 0:fcf070a88ba0 | 284 | servo.calibrate(0.001, 45.0); |
vsutardja | 0:fcf070a88ba0 | 285 | servo = a / 180.0; |
vsutardja | 0:fcf070a88ba0 | 286 | |
vsutardja | 7:8229baaf1fbf | 287 | telemetry_serial.baud(115200); |
vsutardja | 7:8229baaf1fbf | 288 | telemetry_serial.printf("t_h\r\n"); |
vsutardja | 7:8229baaf1fbf | 289 | telemetry_obj.transmit_header(); |
vsutardja | 7:8229baaf1fbf | 290 | |
vsutardja | 0:fcf070a88ba0 | 291 | // Initialize & start camera |
vsutardja | 0:fcf070a88ba0 | 292 | clk = 0; |
vsutardja | 3:c57674c348bd | 293 | read_camera(); |
vsutardja | 0:fcf070a88ba0 | 294 | |
vsutardja | 0:fcf070a88ba0 | 295 | // Initialize motor controller |
vsutardja | 0:fcf070a88ba0 | 296 | motor_ctrl.setInputLimits(0.0, 10.0); |
vsutardja | 4:947c3634b649 | 297 | motor_ctrl.setOutputLimits(0.01, 0.5); |
vsutardja | 4:947c3634b649 | 298 | motor_ctrl.setSetPoint(ref_v); |
vsutardja | 0:fcf070a88ba0 | 299 | motor_ctrl.setBias(0.0); |
vsutardja | 0:fcf070a88ba0 | 300 | motor_ctrl.setMode(1); |
vsutardja | 0:fcf070a88ba0 | 301 | |
vsutardja | 0:fcf070a88ba0 | 302 | // Initialize bluetooth |
vsutardja | 7:8229baaf1fbf | 303 | // bt.baud(115200); |
vsutardja | 0:fcf070a88ba0 | 304 | |
vsutardja | 0:fcf070a88ba0 | 305 | // Initialize communications thread |
vsutardja | 7:8229baaf1fbf | 306 | // Thread communication_thread(communication); |
vsutardja | 7:8229baaf1fbf | 307 | |
vsutardja | 0:fcf070a88ba0 | 308 | |
vsutardja | 0:fcf070a88ba0 | 309 | // Start motor controller |
vsutardja | 0:fcf070a88ba0 | 310 | while (true) { |
vsutardja | 7:8229baaf1fbf | 311 | // telemetry_serial.printf("%d\r\n", t.read_ms()); |
vsutardja | 8:71c39d2f80e2 | 312 | // if (dec == 100) { |
vsutardja | 7:8229baaf1fbf | 313 | tele_time_ms = t.read_ms(); |
vsutardja | 7:8229baaf1fbf | 314 | telemetry_obj.do_io(); |
vsutardja | 8:71c39d2f80e2 | 315 | // dec = 0; |
vsutardja | 8:71c39d2f80e2 | 316 | // } |
vsutardja | 8:71c39d2f80e2 | 317 | // dec = dec + 1; |
vsutardja | 0:fcf070a88ba0 | 318 | curr_pulses = qei.getPulses(); |
vsutardja | 4:947c3634b649 | 319 | //for (int i = 0; i < MVG_AVG - 1; i++) { |
vsutardja | 4:947c3634b649 | 320 | // v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1])); |
vsutardja | 4:947c3634b649 | 321 | // } |
vsutardja | 4:947c3634b649 | 322 | // v_prev[MVG_AVG-1] = velocity; |
vsutardja | 4:947c3634b649 | 323 | velocity = (curr_pulses - prev_pulses)/ interval / ppr * c / 2.5; |
vsutardja | 8:71c39d2f80e2 | 324 | tele_vel = velocity; |
vsutardja | 4:947c3634b649 | 325 | //vel = velocity; |
vsutardja | 4:947c3634b649 | 326 | // for (int i = 0; i < MVG_AVG; i++) { |
vsutardja | 4:947c3634b649 | 327 | // velocity = velocity + v_prev[i]; |
vsutardja | 4:947c3634b649 | 328 | // } |
vsutardja | 4:947c3634b649 | 329 | // velocity = velocity / (MVG_AVG + 1.0); |
vsutardja | 0:fcf070a88ba0 | 330 | prev_pulses = curr_pulses; |
vsutardja | 0:fcf070a88ba0 | 331 | motor_ctrl.setProcessValue(velocity); |
vsutardja | 0:fcf070a88ba0 | 332 | d = motor_ctrl.compute(); |
vsutardja | 0:fcf070a88ba0 | 333 | motor = 1.0 - d; |
vsutardja | 0:fcf070a88ba0 | 334 | wait(interval); |
vsutardja | 7:8229baaf1fbf | 335 | // pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); |
vsutardja | 0:fcf070a88ba0 | 336 | } |
vsutardja | 0:fcf070a88ba0 | 337 | } |