EE192 Team 4 / Mbed 2 deprecated FixedPWMWill

Possibly faster steering response.

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

Fork of FixedPWMWill by Will Porter

main.cpp@24:d206fa06c610, 2016-04-24 (annotated)

Committer:
vsutardja
Date:
Sun Apr 24 23:58:28 2016 +0000
Revision:
24:d206fa06c610
Parent:
22:b9969eaf0e80
Child:
26:722362964784
latest

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;
wretrop 19:296fb2f473a3 29 Timeout expo_time;
vsutardja 18:2b7db50fec4c 30 int t_cam = 0;
vsutardja 12:54e7d8ff3a74 31 int t_steer = 0;
vsutardja 12:54e7d8ff3a74 32 int t_vel = 0;
wretrop 19:296fb2f473a3 33 int t_main = 0;
wretrop 19:296fb2f473a3 34 int t_coms = 0;
vsutardja 12:54e7d8ff3a74 35
wretrop 19:296fb2f473a3 36 float interrupt_T = 0.015; // Something is very wrong here.
vsutardja 14:a0614f48e6ef 37 bool ctrl_flag = false;
vsutardja 0:fcf070a88ba0 38
vsutardja 0:fcf070a88ba0 39 // =============
vsutardja 0:fcf070a88ba0 40 // Communication
vsutardja 0:fcf070a88ba0 41 // =============
vsutardja 12:54e7d8ff3a74 42 char comm_cmd; // Command
vsutardja 18:2b7db50fec4c 43 char comm_in[8]; // Input
vsutardja 18:2b7db50fec4c 44 //Serial bt(USBTX, USBRX); // USB connection
vsutardja 18:2b7db50fec4c 45 Serial bt(PTC4, PTC3); // BlueSMiRF connection
vsutardja 0:fcf070a88ba0 46
vsutardja 12:54e7d8ff3a74 47 void communication(void const *args); // Communications
vsutardja 0:fcf070a88ba0 48
vsutardja 18:2b7db50fec4c 49 //void Kmill(Arguments *input, Reply *output);
vsutardja 18:2b7db50fec4c 50 //RPCFunction rpc_Kmill(&Kmill, "Kmill");
vsutardja 18:2b7db50fec4c 51
vsutardja 0:fcf070a88ba0 52 // =====
vsutardja 0:fcf070a88ba0 53 // Motor
vsutardja 0:fcf070a88ba0 54 // =====
vsutardja 22:b9969eaf0e80 55 const float motor_T = 1.0 / 100; // Frequency
vsutardja 12:54e7d8ff3a74 56 float motor_duty = 0.0; // Duty cycle
vsutardja 22:b9969eaf0e80 57 float ref_pwm = 0.0;
vsutardja 12:54e7d8ff3a74 58 bool e_stop = false; // Emergency stop
vsutardja 24:d206fa06c610 59 InterruptIn bemf_int(PTD4);
vsutardja 24:d206fa06c610 60 PwmOut motor(PTD4); // Enable pin (PWM)
vsutardja 22:b9969eaf0e80 61 Timeout bemf_timeout;
vsutardja 22:b9969eaf0e80 62 int bemf_vel = 0;
vsutardja 22:b9969eaf0e80 63 int motor_ctrl_count = 0;
vsutardja 0:fcf070a88ba0 64
vsutardja 0:fcf070a88ba0 65 // =======
vsutardja 0:fcf070a88ba0 66 // Encoder
vsutardja 0:fcf070a88ba0 67 // =======
vsutardja 12:54e7d8ff3a74 68 const int ppr = 389; // Pulses per revolution
vsutardja 12:54e7d8ff3a74 69 const float c = 0.20106; // Wheel circumference
vsutardja 12:54e7d8ff3a74 70 int prev_pulses = 0; // Previous pulse count
vsutardja 12:54e7d8ff3a74 71 int curr_pulses = 0; // Current pulse count
vsutardja 12:54e7d8ff3a74 72 float velocity = 0; // Velocity
vsutardja 12:54e7d8ff3a74 73 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
vsutardja 0:fcf070a88ba0 74
vsutardja 0:fcf070a88ba0 75 // ========
vsutardja 0:fcf070a88ba0 76 // Velocity
vsutardja 0:fcf070a88ba0 77 // ========
vsutardja 22:b9969eaf0e80 78 float Kmp = 12.0; // Proportional factor
vsutardja 18:2b7db50fec4c 79 float Kmi = 0; // Integral factor
vsutardja 18:2b7db50fec4c 80 float Kmd = 0; // Derivative factor
vsutardja 17:bf6192a361ab 81 float interval = 0.01; // Sampling interval
vsutardja 18:2b7db50fec4c 82 float prev_vels[10];
vsutardja 24:d206fa06c610 83 float ref_v = 0; // Reference velocity
vsutardja 24:d206fa06c610 84 float master_v = 0;
vsutardja 24:d206fa06c610 85 float turn_minv = 1.0;
vsutardja 18:2b7db50fec4c 86 PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
vsutardja 0:fcf070a88ba0 87
vsutardja 22:b9969eaf0e80 88 int turn_thresh = 15;
vsutardja 22:b9969eaf0e80 89 float turn_gain = 0.015;
vsutardja 22:b9969eaf0e80 90 float turn_minpwm = 0.1;
vsutardja 22:b9969eaf0e80 91
vsutardja 22:b9969eaf0e80 92 FastAnalogIn bemf(PTB3, 0);
vsutardja 22:b9969eaf0e80 93
vsutardja 0:fcf070a88ba0 94 // =====
vsutardja 0:fcf070a88ba0 95 // Servo
vsutardja 0:fcf070a88ba0 96 // =====
vsutardja 12:54e7d8ff3a74 97 float angle = 88; // Angle
vsutardja 24:d206fa06c610 98 float Ksp = 0.8; // Steering proportion
vsutardja 24:d206fa06c610 99 float Ksi = 10000000;
vsutardja 24:d206fa06c610 100 float Ksd = 0.0000001;
vsutardja 16:3ab3c4670f4f 101 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
wretrop 20:8f4838b0d94d 102 float Ksp1 = 0.9;
wretrop 20:8f4838b0d94d 103 float Ksi1 = 0;
wretrop 20:8f4838b0d94d 104 float Ksd1 = 0;
wretrop 20:8f4838b0d94d 105 PID servo_ctrl1(Ksp1, Ksi1, Ksd1, interrupt_T);
wretrop 20:8f4838b0d94d 106 int ctrl_switch = 15;
vsutardja 12:54e7d8ff3a74 107 Servo servo(PTA12); // Signal pin (PWM)
vsutardja 0:fcf070a88ba0 108
vsutardja 0:fcf070a88ba0 109 // ======
vsutardja 0:fcf070a88ba0 110 // Camera
vsutardja 0:fcf070a88ba0 111 // ======
vsutardja 12:54e7d8ff3a74 112 int t_int = 10000; // Exposure time
wretrop 19:296fb2f473a3 113 bool rdyFlag = 1; // Signal when camera is ready again
wretrop 19:296fb2f473a3 114 bool dataFlag = 0; // Signal when data is ready again
wretrop 19:296fb2f473a3 115 DigitalOut debug(LED_BLUE);
wretrop 19:296fb2f473a3 116 Timeout debugger;
wretrop 19:296fb2f473a3 117 int fired = 0;
wretrop 19:296fb2f473a3 118
vsutardja 18:2b7db50fec4c 119 const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
vsutardja 12:54e7d8ff3a74 120 const int T_INT_MIN = 35; // Minimum exposure time
vsutardja 12:54e7d8ff3a74 121 int img[108] = {0}; // Image data
vsutardja 24:d206fa06c610 122 DigitalOut clk(PTD0); // Clock pin
vsutardja 24:d206fa06c610 123 DigitalOut si(PTD5); // SI pin
vsutardja 12:54e7d8ff3a74 124 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 125
vsutardja 0:fcf070a88ba0 126 // ================
vsutardja 0:fcf070a88ba0 127 // Image processing
vsutardja 0:fcf070a88ba0 128 // ================
vsutardja 12:54e7d8ff3a74 129 int max = -1; // Maximum luminosity
vsutardja 12:54e7d8ff3a74 130 int left[5] = {0}; // Left edge
vsutardja 12:54e7d8ff3a74 131 int right[5] = {0}; // Right edge
vsutardja 12:54e7d8ff3a74 132 int j = 0; // Peaks index
vsutardja 12:54e7d8ff3a74 133 int center = 64; // Center estimate
vsutardja 12:54e7d8ff3a74 134 int centers[5] = {0}; // Possible centers
vsutardja 12:54e7d8ff3a74 135 int prev_center = 64; // Previous center
vsutardja 12:54e7d8ff3a74 136 float scores[5] = {0}; // Candidate scores
vsutardja 12:54e7d8ff3a74 137 int best_score_idx = 0; // Most likely center index
vsutardja 0:fcf070a88ba0 138
vsutardja 0:fcf070a88ba0 139 // ================
vsutardja 0:fcf070a88ba0 140 // Functions
vsutardja 0:fcf070a88ba0 141 // ================
vsutardja 0:fcf070a88ba0 142
vsutardja 18:2b7db50fec4c 143
vsutardja 18:2b7db50fec4c 144 void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
vsutardja 13:d6e167698517 145 MODSERIAL *serial = q->serial;
vsutardja 13:d6e167698517 146 if (serial->rxGetLastChar() == 'k') {
vsutardja 13:d6e167698517 147 e_stop = true;
vsutardja 13:d6e167698517 148 motor = 1.0;
vsutardja 13:d6e167698517 149 }
vsutardja 15:db95bb7c7f93 150 if (serial->rxGetLastChar() == '+') {
vsutardja 18:2b7db50fec4c 151 ref_v = ref_v + 0.05;
vsutardja 13:d6e167698517 152 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 153 }
vsutardja 13:d6e167698517 154 if (serial->rxGetLastChar() == '-') {
vsutardja 18:2b7db50fec4c 155 ref_v = ref_v - 0.05;
vsutardja 13:d6e167698517 156 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 157 }
vsutardja 13:d6e167698517 158 }
vsutardja 13:d6e167698517 159
vsutardja 12:54e7d8ff3a74 160 // Communications
vsutardja 11:4348bba086a4 161 //void communication(void const *args) {
vsutardja 14:a0614f48e6ef 162 // telemetry_serial.baud(115200);
vsutardja 18:2b7db50fec4c 163 // telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
vsutardja 14:a0614f48e6ef 164 // telemetry_obj.transmit_header();
vsutardja 11:4348bba086a4 165 // while (true) {
vsutardja 14:a0614f48e6ef 166 // tele_time_ms = t_tele.read_ms();
vsutardja 14:a0614f48e6ef 167 // telemetry_obj.do_io();
vsutardja 11:4348bba086a4 168 // }
vsutardja 11:4348bba086a4 169 //}
vsutardja 11:4348bba086a4 170
vsutardja 14:a0614f48e6ef 171 void communication(void const *args) {
vsutardja 14:a0614f48e6ef 172 // Initialize bluetooth
vsutardja 14:a0614f48e6ef 173 bt.baud(115200);
vsutardja 14:a0614f48e6ef 174
vsutardja 14:a0614f48e6ef 175 while (true) {
wretrop 19:296fb2f473a3 176
vsutardja 14:a0614f48e6ef 177 bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 14:a0614f48e6ef 178 bt.printf("Available diagnostics:\r\n");
vsutardja 14:a0614f48e6ef 179 bt.printf(" [0] Velocity\r\n");
vsutardja 14:a0614f48e6ef 180 bt.printf(" [1] Steering\r\n");
vsutardja 18:2b7db50fec4c 181 // bt.printf(" [2] Change Kmp\r\n");
vsutardja 18:2b7db50fec4c 182 // bt.printf(" [3] Change Kmi\r\n");
vsutardja 18:2b7db50fec4c 183 // bt.printf(" [4] Change Kmd\r\n");
vsutardja 18:2b7db50fec4c 184 bt.printf(" [2] Change Ksp\r\n");
vsutardja 18:2b7db50fec4c 185 bt.printf(" [3] Change Ksi\r\n");
vsutardja 18:2b7db50fec4c 186 bt.printf(" [4] Change Ksd\r\n");
vsutardja 22:b9969eaf0e80 187 // bt.printf(" [5] Change Ksp1\r\n");
vsutardja 24:d206fa06c610 188 bt.printf(" [5] Change turn slowing minimum velocity\r\n");
wretrop 20:8f4838b0d94d 189 // bt.printf(" [6] Change reference velocity\r\n");
vsutardja 22:b9969eaf0e80 190 bt.printf(" [6] Change turn slowing gain\r\n");
vsutardja 22:b9969eaf0e80 191 // bt.printf(" [7] EMERGENCY STOP\r\n");
vsutardja 22:b9969eaf0e80 192 bt.printf(" [7] Change turn slowing dead zone\r\n");
vsutardja 18:2b7db50fec4c 193 // bt.printf(" [8] Timing\r\n");
vsutardja 24:d206fa06c610 194 bt.printf(" [8] master_v += 0.05\r\n");
vsutardja 24:d206fa06c610 195 bt.printf(" [9] master_v -= 0.05\r\n");
vsutardja 14:a0614f48e6ef 196 comm_cmd = bt.getc();
vsutardja 14:a0614f48e6ef 197 while (comm_cmd != 'q') {
wretrop 19:296fb2f473a3 198 t.reset();
wretrop 19:296fb2f473a3 199 t.start();
vsutardja 14:a0614f48e6ef 200 switch(atoi(&comm_cmd)){
vsutardja 14:a0614f48e6ef 201 case 0:
vsutardja 22:b9969eaf0e80 202 bt.printf("bemf: %d, Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", bemf_vel, motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
vsutardja 14:a0614f48e6ef 203 break;
vsutardja 14:a0614f48e6ef 204 case 1:
wretrop 19:296fb2f473a3 205 bt.printf("Servo angle: %f, Track center: %d, t_int: %d,rdyFlag: %d, dataFlag: %d, fired: %d, timer: %d\r\n", angle, center, t_int,rdyFlag,dataFlag,fired,t_coms);
vsutardja 14:a0614f48e6ef 206 break;
vsutardja 14:a0614f48e6ef 207 case 2:
vsutardja 18:2b7db50fec4c 208 bt.printf("Current: %f, New (8 digits): ", Ksp);
vsutardja 18:2b7db50fec4c 209 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 210 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 211 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 212 }
vsutardja 14:a0614f48e6ef 213 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 214 Ksp = atof(comm_in);
vsutardja 18:2b7db50fec4c 215 servo_ctrl.setTunings(Ksp, Ksi, Ksd);
wretrop 20:8f4838b0d94d 216 servo_ctrl.reset();
vsutardja 14:a0614f48e6ef 217 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 218 break;
vsutardja 14:a0614f48e6ef 219 case 3:
vsutardja 18:2b7db50fec4c 220 bt.printf("Current: %f, New (8 digits): ", Ksi);
vsutardja 18:2b7db50fec4c 221 for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 222 comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 223 bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 224 }
vsutardja 18:2b7db50fec4c 225 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 226 Ksi = atof(comm_in);
wretrop 20:8f4838b0d94d 227 servo_ctrl.setTunings(Ksp, Ksi, Ksd);
wretrop 20:8f4838b0d94d 228 servo_ctrl.reset();
vsutardja 18:2b7db50fec4c 229 comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 230 break;
vsutardja 18:2b7db50fec4c 231 case 4:
vsutardja 18:2b7db50fec4c 232 bt.printf("Current: %f, New (8 digits): ", Ksd);
vsutardja 18:2b7db50fec4c 233 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 234 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 235 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 236 }
vsutardja 14:a0614f48e6ef 237 bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 238 Ksd = atof(comm_in);
wretrop 20:8f4838b0d94d 239 servo_ctrl.setTunings(Ksp, Ksi, Ksd);
wretrop 20:8f4838b0d94d 240 servo_ctrl.reset();
vsutardja 14:a0614f48e6ef 241 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 242 break;
vsutardja 18:2b7db50fec4c 243 // case 2:
vsutardja 18:2b7db50fec4c 244 // bt.printf("Current: %f, New (8 digits): ", Kmp);
vsutardja 18:2b7db50fec4c 245 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 246 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 247 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 248 // }
vsutardja 18:2b7db50fec4c 249 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 250 // Kmp = atof(comm_in);
vsutardja 18:2b7db50fec4c 251 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 252 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 253 // break;
vsutardja 18:2b7db50fec4c 254 // case 3:
vsutardja 18:2b7db50fec4c 255 // bt.printf("Current: %f, New (8 digits): ", Kmi);
vsutardja 18:2b7db50fec4c 256 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 257 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 258 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 259 // }
vsutardja 18:2b7db50fec4c 260 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 261 // Kmi = atof(comm_in);
vsutardja 18:2b7db50fec4c 262 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 263 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 264 // break;
vsutardja 18:2b7db50fec4c 265 // case 4:
vsutardja 18:2b7db50fec4c 266 // bt.printf("Current: %f, New (8 digits): ", Kmd);
vsutardja 18:2b7db50fec4c 267 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 268 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 269 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 270 // }
vsutardja 18:2b7db50fec4c 271 // bt.printf("\r\n");
vsutardja 18:2b7db50fec4c 272 // Kmd = atof(comm_in);
vsutardja 18:2b7db50fec4c 273 // motor_ctrl.setTunings(Kmp, Kmi, Kmd);
vsutardja 18:2b7db50fec4c 274 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 275 // break;
vsutardja 22:b9969eaf0e80 276 // case 5:
vsutardja 22:b9969eaf0e80 277 // bt.printf("Current: %f, New (8 digits): ", Ksp1);
vsutardja 22:b9969eaf0e80 278 // for (int i = 0; i < 8; i++) {
vsutardja 22:b9969eaf0e80 279 // comm_in[i] = bt.getc();
vsutardja 22:b9969eaf0e80 280 // bt.putc(comm_in[i]);
vsutardja 22:b9969eaf0e80 281 // }
vsutardja 22:b9969eaf0e80 282 // bt.printf("\r\n");
vsutardja 22:b9969eaf0e80 283 // Ksp1 = atof(comm_in);
vsutardja 22:b9969eaf0e80 284 // servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
vsutardja 22:b9969eaf0e80 285 // comm_cmd = 'q';
vsutardja 22:b9969eaf0e80 286 // break;
vsutardja 14:a0614f48e6ef 287 case 5:
vsutardja 24:d206fa06c610 288 bt.printf("Current: %d, New (8 digits): ", turn_minv);
vsutardja 18:2b7db50fec4c 289 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 290 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 291 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 292 }
vsutardja 14:a0614f48e6ef 293 bt.printf("\r\n");
vsutardja 24:d206fa06c610 294 turn_minv = atoi(comm_in);
vsutardja 14:a0614f48e6ef 295 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 296 break;
vsutardja 14:a0614f48e6ef 297 case 6:
vsutardja 22:b9969eaf0e80 298 bt.printf("Current: %d, New (8 digits): ", turn_gain);
vsutardja 18:2b7db50fec4c 299 for (int i = 0; i < 8; i++) {
vsutardja 14:a0614f48e6ef 300 comm_in[i] = bt.getc();
vsutardja 14:a0614f48e6ef 301 bt.putc(comm_in[i]);
vsutardja 14:a0614f48e6ef 302 }
vsutardja 14:a0614f48e6ef 303 bt.printf("\r\n");
vsutardja 22:b9969eaf0e80 304 turn_gain = atoi(comm_in);
vsutardja 14:a0614f48e6ef 305 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 306 break;
wretrop 20:8f4838b0d94d 307 // case 6:
wretrop 20:8f4838b0d94d 308 // bt.printf("Current: %f, New (8 digits): ", ref_v);
wretrop 20:8f4838b0d94d 309 // for (int i = 0; i < 8; i++) {
wretrop 20:8f4838b0d94d 310 // comm_in[i] = bt.getc();
wretrop 20:8f4838b0d94d 311 // bt.putc(comm_in[i]);
wretrop 20:8f4838b0d94d 312 // }
wretrop 20:8f4838b0d94d 313 // bt.printf("\r\n");
wretrop 20:8f4838b0d94d 314 // ref_v = atof(comm_in);
wretrop 20:8f4838b0d94d 315 // motor_ctrl.setSetPoint(ref_v);
wretrop 20:8f4838b0d94d 316 // comm_cmd = 'q';
wretrop 20:8f4838b0d94d 317 // break;
vsutardja 22:b9969eaf0e80 318 // case 7:
vsutardja 18:2b7db50fec4c 319 // e_stop = true;
vsutardja 22:b9969eaf0e80 320 // motor = 1.0;
vsutardja 22:b9969eaf0e80 321 // bt.printf("STOPPED\r\n");
vsutardja 22:b9969eaf0e80 322 // comm_cmd = 'q';
vsutardja 22:b9969eaf0e80 323 // break;
vsutardja 22:b9969eaf0e80 324 case 7:
vsutardja 22:b9969eaf0e80 325 bt.printf("Current: %d, New (8 digits): ", turn_thresh);
vsutardja 22:b9969eaf0e80 326 for (int i = 0; i < 8; i++) {
vsutardja 22:b9969eaf0e80 327 comm_in[i] = bt.getc();
vsutardja 22:b9969eaf0e80 328 bt.putc(comm_in[i]);
vsutardja 22:b9969eaf0e80 329 }
vsutardja 22:b9969eaf0e80 330 bt.printf("\r\n");
vsutardja 22:b9969eaf0e80 331 turn_thresh = atoi(comm_in);
vsutardja 14:a0614f48e6ef 332 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 333 break;
vsutardja 18:2b7db50fec4c 334 // case 8:
vsutardja 18:2b7db50fec4c 335 // 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 336 // break;
vsutardja 14:a0614f48e6ef 337 case 8:
vsutardja 24:d206fa06c610 338 master_v = master_v + 0.25;
vsutardja 24:d206fa06c610 339 motor_ctrl.setSetPoint(master_v);
vsutardja 22:b9969eaf0e80 340 // motor_duty = motor_duty + 0.05;
vsutardja 22:b9969eaf0e80 341 // ref_pwm = ref_pwm + 0.05;
vsutardja 22:b9969eaf0e80 342 // motor = 1.0 - motor_duty;
vsutardja 22:b9969eaf0e80 343 // bt.printf("%f\r\n", motor_duty);
vsutardja 22:b9969eaf0e80 344 // bt.printf("%f\r\n", ref_pwm);
vsutardja 24:d206fa06c610 345 bt.printf("%f\r\n", master_v);
vsutardja 18:2b7db50fec4c 346 comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 347 break;
vsutardja 18:2b7db50fec4c 348 case 9:
vsutardja 24:d206fa06c610 349 master_v = master_v - 0.25;
vsutardja 24:d206fa06c610 350 motor_ctrl.setSetPoint(master_v);
vsutardja 22:b9969eaf0e80 351 // motor_duty = motor_duty - 0.05;
vsutardja 22:b9969eaf0e80 352 // ref_pwm = ref_pwm - 0.05;
vsutardja 22:b9969eaf0e80 353 // motor = 1.0 - motor_duty;
vsutardja 22:b9969eaf0e80 354 // bt.printf("%f\r\n", motor_duty);
vsutardja 22:b9969eaf0e80 355 // bt.printf("%f\r\n", ref_pwm);
vsutardja 24:d206fa06c610 356 bt.printf("%f\r\n", master_v);
vsutardja 18:2b7db50fec4c 357 comm_cmd = 'q';
vsutardja 14:a0614f48e6ef 358 break;
vsutardja 14:a0614f48e6ef 359 }
vsutardja 14:a0614f48e6ef 360 if (bt.readable()) {
vsutardja 14:a0614f48e6ef 361 comm_cmd = bt.getc();
vsutardja 14:a0614f48e6ef 362 }
wretrop 19:296fb2f473a3 363 t_coms = t.read_us();
vsutardja 14:a0614f48e6ef 364 }
wretrop 19:296fb2f473a3 365
vsutardja 14:a0614f48e6ef 366 }
vsutardja 14:a0614f48e6ef 367 }
vsutardja 14:a0614f48e6ef 368
wretrop 19:296fb2f473a3 369 void bugger(){
wretrop 19:296fb2f473a3 370 debug = !debug;
wretrop 19:296fb2f473a3 371 }
wretrop 19:296fb2f473a3 372 void cam_Read(){
vsutardja 24:d206fa06c610 373 PTD->PCOR = (0x01);
vsutardja 24:d206fa06c610 374 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 375 PTD->PSOR = (0x01);
vsutardja 24:d206fa06c610 376 PTD->PCOR = (0x01 << 5);
vsutardja 0:fcf070a88ba0 377
vsutardja 0:fcf070a88ba0 378 for (int i = 0; i < 128; i++) {
vsutardja 24:d206fa06c610 379 PTD->PCOR = (0x01);
vsutardja 12:54e7d8ff3a74 380 if (i > 9 && i < 118) {
vsutardja 12:54e7d8ff3a74 381 img[i-10] = ao.read_u16();
vsutardja 12:54e7d8ff3a74 382 tele_linescan[i-10] = img[i-10];
vsutardja 12:54e7d8ff3a74 383 }
vsutardja 24:d206fa06c610 384 PTD->PSOR = (0x01);
vsutardja 0:fcf070a88ba0 385 }
wretrop 19:296fb2f473a3 386 dataFlag = 1;
wretrop 19:296fb2f473a3 387 debugger.attach(bugger,0.2);
wretrop 19:296fb2f473a3 388 fired ++;
wretrop 19:296fb2f473a3 389 }
wretrop 19:296fb2f473a3 390
wretrop 19:296fb2f473a3 391 void control() {
wretrop 19:296fb2f473a3 392 // Image capture
wretrop 19:296fb2f473a3 393 //t.reset();
wretrop 19:296fb2f473a3 394 //t.start();
wretrop 19:296fb2f473a3 395 DigitalOut debug2(LED_GREEN);
wretrop 19:296fb2f473a3 396 debug2 = 1;
wretrop 19:296fb2f473a3 397 // Dummy read
wretrop 19:296fb2f473a3 398 if (rdyFlag){
vsutardja 24:d206fa06c610 399 PTD->PCOR = (0x01);
vsutardja 24:d206fa06c610 400 PTD->PSOR = (0x01 << 5);
wretrop 19:296fb2f473a3 401 PTD->PSOR = (0x01);
vsutardja 24:d206fa06c610 402 PTD->PCOR = (0x01 << 5);
vsutardja 0:fcf070a88ba0 403
wretrop 19:296fb2f473a3 404 for (int i = 0; i < 128; i++) {
vsutardja 24:d206fa06c610 405 PTD->PCOR = (0x01);
vsutardja 24:d206fa06c610 406 PTD->PSOR = (0x01);
wretrop 19:296fb2f473a3 407 }
wretrop 19:296fb2f473a3 408
wretrop 19:296fb2f473a3 409 // Expose
wretrop 19:296fb2f473a3 410 expo_time.attach_us(&cam_Read,t_int);
wretrop 19:296fb2f473a3 411 rdyFlag = 0;
wretrop 19:296fb2f473a3 412
wretrop 19:296fb2f473a3 413 }
wretrop 19:296fb2f473a3 414
vsutardja 0:fcf070a88ba0 415
vsutardja 12:54e7d8ff3a74 416 // Detect peak edges
wretrop 19:296fb2f473a3 417 if(dataFlag){
wretrop 19:296fb2f473a3 418 j = 0;
wretrop 19:296fb2f473a3 419 for (int i = 0; i < 107 && j < 5; i++) {
wretrop 19:296fb2f473a3 420 if (img[i] > 45000) {
wretrop 19:296fb2f473a3 421 left[j] = i;
wretrop 19:296fb2f473a3 422 while (img[i] > 45000) {
wretrop 19:296fb2f473a3 423 i = i + 1;
wretrop 19:296fb2f473a3 424 }
wretrop 19:296fb2f473a3 425 right[j] = i;
wretrop 19:296fb2f473a3 426 j = j + 1;
vsutardja 0:fcf070a88ba0 427 }
wretrop 19:296fb2f473a3 428 }
wretrop 19:296fb2f473a3 429
wretrop 19:296fb2f473a3 430 // Calculate peak centers
wretrop 19:296fb2f473a3 431 for (int i = 0; i < j; i++) {
wretrop 19:296fb2f473a3 432 centers[i] = (left[i] + right[i] + 20) / 2;
wretrop 19:296fb2f473a3 433 }
wretrop 19:296fb2f473a3 434
wretrop 19:296fb2f473a3 435 // Assign scores
wretrop 19:296fb2f473a3 436 for (int i = 0; i < j; i++) {
wretrop 19:296fb2f473a3 437 scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
wretrop 19:296fb2f473a3 438 }
wretrop 19:296fb2f473a3 439
wretrop 19:296fb2f473a3 440 // Choose most likely center
wretrop 19:296fb2f473a3 441 best_score_idx = 0;
wretrop 19:296fb2f473a3 442 for (int i = 0; i < j; i++) {
wretrop 19:296fb2f473a3 443 if (scores[i] > scores[best_score_idx]) {
wretrop 19:296fb2f473a3 444 best_score_idx = i;
wretrop 19:296fb2f473a3 445 }
vsutardja 0:fcf070a88ba0 446 }
wretrop 19:296fb2f473a3 447 prev_center = center;
wretrop 19:296fb2f473a3 448 center = centers[best_score_idx];
wretrop 19:296fb2f473a3 449 tele_center = center;
wretrop 19:296fb2f473a3 450
wretrop 19:296fb2f473a3 451 // Set servo angle
wretrop 19:296fb2f473a3 452 //angle = 88 + (55 - center) * Ksp;
wretrop 19:296fb2f473a3 453 // if (angle > 113) {
wretrop 19:296fb2f473a3 454 // angle = 113;
wretrop 19:296fb2f473a3 455 // }
wretrop 19:296fb2f473a3 456 // if (angle < 63) {
wretrop 19:296fb2f473a3 457 // angle = 63;
wretrop 19:296fb2f473a3 458 // }
wretrop 19:296fb2f473a3 459 // servo = angle / 180;
wretrop 20:8f4838b0d94d 460
wretrop 20:8f4838b0d94d 461 // if (abs(center - 64) > ctrl_switch) {
wretrop 20:8f4838b0d94d 462 // servo_ctrl1.setProcessValue(center);
wretrop 20:8f4838b0d94d 463 // angle = 88+servo_ctrl1.compute();
wretrop 20:8f4838b0d94d 464 // servo_ctrl.reset();
wretrop 20:8f4838b0d94d 465 // } else {
wretrop 20:8f4838b0d94d 466 servo_ctrl.setProcessValue(center);
wretrop 20:8f4838b0d94d 467 angle = 88 + servo_ctrl.compute();
wretrop 20:8f4838b0d94d 468 // servo_ctrl1.reset();
wretrop 20:8f4838b0d94d 469 // }
wretrop 19:296fb2f473a3 470 servo = angle / 180;
wretrop 19:296fb2f473a3 471
vsutardja 22:b9969eaf0e80 472
wretrop 19:296fb2f473a3 473 // AGC
wretrop 19:296fb2f473a3 474 max = -1;
wretrop 19:296fb2f473a3 475 for (int i = 0; i < 107; i++) {
wretrop 19:296fb2f473a3 476 if (img[i] > max) {
wretrop 19:296fb2f473a3 477 max = img[i];
wretrop 19:296fb2f473a3 478 }
vsutardja 12:54e7d8ff3a74 479 }
wretrop 19:296fb2f473a3 480 if (max > 60000) {
wretrop 19:296fb2f473a3 481 t_int = t_int - 0.1 * (max - 60000);
wretrop 19:296fb2f473a3 482 }
wretrop 19:296fb2f473a3 483 if (max < 50000) {
wretrop 19:296fb2f473a3 484 t_int = t_int + 0.1 * (50000 - max);
wretrop 19:296fb2f473a3 485 }
wretrop 19:296fb2f473a3 486 if (t_int < T_INT_MIN) {
wretrop 19:296fb2f473a3 487 t_int = T_INT_MIN;
wretrop 19:296fb2f473a3 488 }
wretrop 19:296fb2f473a3 489 if (t_int > T_INT_MAX) {
wretrop 19:296fb2f473a3 490 t_int = T_INT_MAX;
wretrop 19:296fb2f473a3 491 }
wretrop 19:296fb2f473a3 492 tele_exposure = t_int;
wretrop 20:8f4838b0d94d 493 servo_ctrl.setInterval(t_int);
wretrop 19:296fb2f473a3 494 rdyFlag = 1;
wretrop 19:296fb2f473a3 495 dataFlag = 0;
wretrop 19:296fb2f473a3 496 //t_main = t.read_us();
vsutardja 12:54e7d8ff3a74 497 }
vsutardja 22:b9969eaf0e80 498
vsutardja 22:b9969eaf0e80 499 if (motor_ctrl_count == 1000) {
vsutardja 24:d206fa06c610 500 velocity = (40000 - bemf_vel) / 6000.0;
vsutardja 22:b9969eaf0e80 501 // motor_duty = motor_duty + 0.1;
vsutardja 22:b9969eaf0e80 502 // motor = 1.0 - motor_duty;
vsutardja 22:b9969eaf0e80 503 motor_ctrl_count = 0;
vsutardja 22:b9969eaf0e80 504 motor_ctrl.setProcessValue(velocity);
vsutardja 22:b9969eaf0e80 505 motor_duty = motor_ctrl.compute();
vsutardja 24:d206fa06c610 506 motor = motor_duty;
vsutardja 22:b9969eaf0e80 507 motor_ctrl_count = 0;
vsutardja 22:b9969eaf0e80 508 // }
vsutardja 22:b9969eaf0e80 509 // motor_ctrl_count = motor_ctrl_count + 1;
vsutardja 22:b9969eaf0e80 510 } else {
vsutardja 22:b9969eaf0e80 511 motor_ctrl_count = motor_ctrl_count + 1;
vsutardja 22:b9969eaf0e80 512 }
vsutardja 22:b9969eaf0e80 513
vsutardja 24:d206fa06c610 514 ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
vsutardja 24:d206fa06c610 515 if (ref_v < turn_minv) {
vsutardja 24:d206fa06c610 516 ref_v = turn_minv;
vsutardja 24:d206fa06c610 517 }
vsutardja 24:d206fa06c610 518 if (ref_v > master_v) {
vsutardja 24:d206fa06c610 519 ref_v = master_v;
vsutardja 24:d206fa06c610 520 }
vsutardja 24:d206fa06c610 521 if (abs(motor_ctrl.getSetPoint() - ref_v) > 0.05) {
vsutardja 24:d206fa06c610 522 motor_ctrl.setSetPoint(ref_v);
vsutardja 24:d206fa06c610 523 }
vsutardja 24:d206fa06c610 524
vsutardja 22:b9969eaf0e80 525 // motor_duty = ref_pwm - ref_pwm * turn_gain * (abs(64 - center) - turn_thresh);
vsutardja 22:b9969eaf0e80 526 // if (motor_duty > ref_pwm) {
vsutardja 22:b9969eaf0e80 527 // motor_duty = ref_pwm;
vsutardja 22:b9969eaf0e80 528 // }
vsutardja 22:b9969eaf0e80 529 // if (motor_duty < turn_minpwm) {
vsutardja 22:b9969eaf0e80 530 // motor_duty = turn_minpwm;
vsutardja 22:b9969eaf0e80 531 // }
vsutardja 22:b9969eaf0e80 532 // if (abs(1.0 - motor.read() - motor_duty) > 0.01) {
vsutardja 22:b9969eaf0e80 533 // motor = 1.0 - motor_duty;
vsutardja 22:b9969eaf0e80 534 // }
vsutardja 22:b9969eaf0e80 535
vsutardja 18:2b7db50fec4c 536 // t_steer = t.read_us();
vsutardja 11:4348bba086a4 537
vsutardja 22:b9969eaf0e80 538
vsutardja 22:b9969eaf0e80 539
vsutardja 18:2b7db50fec4c 540 // // Velocity control
vsutardja 18:2b7db50fec4c 541 // // t.reset();
vsutardja 18:2b7db50fec4c 542 // if (!e_stop) {
vsutardja 18:2b7db50fec4c 543 // curr_pulses = qei.getPulses();
vsutardja 18:2b7db50fec4c 544 // //for (int i = 0; i < 9; i++) {
vsutardja 18:2b7db50fec4c 545 //// prev_vels[i] = prev_vels[i+1];
vsutardja 18:2b7db50fec4c 546 //// }
vsutardja 18:2b7db50fec4c 547 //// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
vsutardja 18:2b7db50fec4c 548 //// t.reset();
vsutardja 18:2b7db50fec4c 549 //// if (prev_vels[9] < 0) {
vsutardja 18:2b7db50fec4c 550 //// prev_vels[9] = 0;
vsutardja 18:2b7db50fec4c 551 //// }
vsutardja 18:2b7db50fec4c 552 //// if (prev_vels[0] < 0) {
vsutardja 18:2b7db50fec4c 553 //// velocity = prev_vels[9];
vsutardja 18:2b7db50fec4c 554 //// } else {
vsutardja 18:2b7db50fec4c 555 //// velocity = 0;
vsutardja 18:2b7db50fec4c 556 //// for (int i = 0; i < 10; i++) {
vsutardja 18:2b7db50fec4c 557 //// velocity = velocity + prev_vels[i];
vsutardja 18:2b7db50fec4c 558 //// velocity = velocity / 10;
vsutardja 18:2b7db50fec4c 559 //// }
vsutardja 18:2b7db50fec4c 560 //// }
vsutardja 18:2b7db50fec4c 561 // velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
vsutardja 18:2b7db50fec4c 562 // if (velocity < 0) {
vsutardja 18:2b7db50fec4c 563 // velocity = 0;
vsutardja 18:2b7db50fec4c 564 // }
vsutardja 18:2b7db50fec4c 565 //// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
vsutardja 18:2b7db50fec4c 566 // t.reset();
vsutardja 18:2b7db50fec4c 567 // tele_velocity = velocity;
vsutardja 18:2b7db50fec4c 568 // prev_pulses = curr_pulses;
vsutardja 18:2b7db50fec4c 569 // motor_ctrl.setProcessValue(velocity);
vsutardja 18:2b7db50fec4c 570 // motor_duty = motor_ctrl.compute();
vsutardja 18:2b7db50fec4c 571 // motor = 1.0 - motor_duty;
vsutardja 18:2b7db50fec4c 572 // tele_pwm = motor_duty;
vsutardja 18:2b7db50fec4c 573 // } else {
vsutardja 18:2b7db50fec4c 574 // motor = 1.0;
vsutardja 18:2b7db50fec4c 575 // }
vsutardja 18:2b7db50fec4c 576 // // t_vel = t.read_us();
vsutardja 18:2b7db50fec4c 577 // ctrl_flag = false;
vsutardja 14:a0614f48e6ef 578 }
vsutardja 14:a0614f48e6ef 579
vsutardja 18:2b7db50fec4c 580 void set_control_flag() {
vsutardja 14:a0614f48e6ef 581 ctrl_flag = true;
vsutardja 0:fcf070a88ba0 582 }
vsutardja 0:fcf070a88ba0 583
vsutardja 22:b9969eaf0e80 584 void meas_bemf() {
vsutardja 22:b9969eaf0e80 585 bemf_vel = bemf.read_u16();
vsutardja 22:b9969eaf0e80 586 }
vsutardja 22:b9969eaf0e80 587
vsutardja 22:b9969eaf0e80 588 void bemf_irq() {
vsutardja 22:b9969eaf0e80 589 bemf_timeout.attach(&meas_bemf, 0.002);
vsutardja 22:b9969eaf0e80 590 }
vsutardja 22:b9969eaf0e80 591
vsutardja 0:fcf070a88ba0 592 // ====
vsutardja 0:fcf070a88ba0 593 // Main
vsutardja 0:fcf070a88ba0 594 // ====
vsutardja 0:fcf070a88ba0 595 int main() {
wretrop 19:296fb2f473a3 596 // t.start();
vsutardja 12:54e7d8ff3a74 597 t_tele.start();
vsutardja 12:54e7d8ff3a74 598 tele_center.set_limits(0, 128);
vsutardja 12:54e7d8ff3a74 599 tele_pwm.set_limits(0.0, 1.0);
vsutardja 2:a8adff46eaca 600
vsutardja 22:b9969eaf0e80 601 bemf_int.fall(&bemf_irq);
vsutardja 22:b9969eaf0e80 602
vsutardja 18:2b7db50fec4c 603 for (int i = 0; i < 10; i++) {
vsutardja 18:2b7db50fec4c 604 prev_vels[i] = -1;
vsutardja 18:2b7db50fec4c 605 }
vsutardja 18:2b7db50fec4c 606
vsutardja 0:fcf070a88ba0 607 // Initialize motor
vsutardja 22:b9969eaf0e80 608 motor.period(motor_T);
vsutardja 24:d206fa06c610 609 motor = motor_duty;
vsutardja 0:fcf070a88ba0 610
vsutardja 0:fcf070a88ba0 611 // Initialize motor controller
vsutardja 0:fcf070a88ba0 612 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 22:b9969eaf0e80 613 motor_ctrl.setOutputLimits(0.05, 0.75);
vsutardja 24:d206fa06c610 614 motor_ctrl.setSetPoint(master_v);
vsutardja 0:fcf070a88ba0 615 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 616 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 617
vsutardja 12:54e7d8ff3a74 618 // Initialize servo
vsutardja 12:54e7d8ff3a74 619 servo.calibrate(0.001, 45.0);
vsutardja 12:54e7d8ff3a74 620 servo = angle / 180.0;
vsutardja 0:fcf070a88ba0 621
vsutardja 18:2b7db50fec4c 622 servo_ctrl.setInputLimits(10, 107);
wretrop 19:296fb2f473a3 623 //servo_ctrl.setOutputLimits(-25, 25);
wretrop 19:296fb2f473a3 624 servo_ctrl.setOutputLimits(-30, 30);
vsutardja 16:3ab3c4670f4f 625 servo_ctrl.setSetPoint(63.5);
vsutardja 18:2b7db50fec4c 626 servo_ctrl.setBias(0.0);
vsutardja 16:3ab3c4670f4f 627 servo_ctrl.setMode(1);
vsutardja 16:3ab3c4670f4f 628
wretrop 20:8f4838b0d94d 629 servo_ctrl1.setInputLimits(10, 107);
wretrop 20:8f4838b0d94d 630 //servo_ctrl1.setOutputLimits(-25, 25);
wretrop 20:8f4838b0d94d 631 servo_ctrl1.setOutputLimits(-30, 30);
wretrop 20:8f4838b0d94d 632 servo_ctrl1.setSetPoint(63.5);
wretrop 20:8f4838b0d94d 633 servo_ctrl1.setBias(0.0);
wretrop 20:8f4838b0d94d 634 servo_ctrl1.setMode(1);
wretrop 20:8f4838b0d94d 635
vsutardja 0:fcf070a88ba0 636 // Initialize communications thread
vsutardja 18:2b7db50fec4c 637 Thread communication_thread(communication);
vsutardja 18:2b7db50fec4c 638
wretrop 19:296fb2f473a3 639 // control_interrupt.attach(control, interrupt_T);
vsutardja 18:2b7db50fec4c 640 // control_interrupt.attach(set_control_flag, interrupt_T);
vsutardja 12:54e7d8ff3a74 641
vsutardja 14:a0614f48e6ef 642 while (true) {
wretrop 19:296fb2f473a3 643 control();
wretrop 19:296fb2f473a3 644 }
vsutardja 0:fcf070a88ba0 645 }