EE192 Team 4
Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
FixedPWMWill
by 
Will Porter
Revision 28:4b76abe148cd, committed 2016-04-27
- Comitter:
- vsutardja
- Date:
- Wed Apr 27 22:24:11 2016 +0000
- Parent:
- 27:e796e9ee0495
- Commit message:
- trying telemetry again
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Apr 26 06:11:46 2016 +0000
+++ b/main.cpp Wed Apr 27 22:24:11 2016 +0000
@@ -7,6 +7,11 @@
#include "Servo.h"
#include "telemetry.h"
+#define set_clk() PTD->PSOR = (0x01);
+#define clr_clk() PTD->PCOR = (0x01);
+#define set_si() PTD->PSOR = (0x01 << 5);
+#define clr_si() PTD->PCOR = (0x01 << 5);
+
// =========
// Telemetry
@@ -18,23 +23,13 @@
telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
-telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0);
+telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);
-telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0);
-telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
-Timer t;
Timer t_tele;
-Ticker control_interrupt;
Timeout expo_time;
-int t_cam = 0;
-int t_steer = 0;
-int t_vel = 0;
-int t_main = 0;
-int t_coms = 0;
float interrupt_T = 0.015; // Something is very wrong here.
-bool ctrl_flag = false;
// =============
// Communication
@@ -46,76 +41,46 @@
void communication(void const *args); // Communications
-//void Kmill(Arguments *input, Reply *output);
-//RPCFunction rpc_Kmill(&Kmill, "Kmill");
-
// =====
// Motor
// =====
-const float motor_T = 1.0 / 100; // Frequency
+const float motor_T = 1.0 / 100; // Frequency
float motor_duty = 0.0; // Duty cycle
-float ref_pwm = 0.0;
bool e_stop = false; // Emergency stop
-InterruptIn bemf_int(PTD4);
+InterruptIn bemf_int(PTD4); // Back EMF measurement trigger
PwmOut motor(PTD4); // Enable pin (PWM)
-Timeout bemf_timeout;
-int bemf_vel = 0;
-int motor_ctrl_count = 0;
-
-// =======
-// Encoder
-// =======
-const int ppr = 389; // Pulses per revolution
-const float c = 0.20106; // Wheel circumference
-int prev_pulses = 0; // Previous pulse count
-int curr_pulses = 0; // Current pulse count
+Timeout bemf_timeout; // Back EMF interrupt delay
+FastAnalogIn bemf(PTB3, 0); // Back EMF read pin
+int bemf_vel = 0; // Back EMF reading
float velocity = 0; // Velocity
-QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
-
-// ========
-// Velocity
-// ========
-float Kmp = 12.0; // Proportional factor
-float Kmi = 0; // Integral factor
-float Kmd = 0; // Derivative factor
-float interval = 0.01; // Sampling interval
-float prev_vels[10];
-float ref_v = 0; // Reference velocity
-float master_v = 0;
-float turn_minv = 1.0;
-PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
-
-int turn_thresh = 19;
-float turn_gain = 0.06;
-float turn_minpwm = 0.1;
-
-FastAnalogIn bemf(PTB3, 0);
+int motor_ctrl_count = 0; // Velocity control decimation counter
+float Kmp = 12.0; // Proportional factor
+float Kmi = 0; // Integral factor
+float Kmd = 0; // Derivative factor
+float master_v = 0; // Master velocity
+float ref_v = 0; // Reference velocity
+PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
+int turn_thresh = 19; // Minimum error to determine turn
+float turn_gain = 0.06; // Proportional gain for turning reference velocity
+float turn_minv = 1.0; // Minimum turning velocity
// =====
// Servo
// =====
float angle = 88; // Angle
-float Ksp = 1.5; // Steering proportion
-float Ksi = 9000000;
-float Ksd = 1.2;
-PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
-float Ksp1 = 0.9;
-float Ksi1 = 0;
-float Ksd1 = 0;
-PID servo_ctrl1(Ksp1, Ksi1, Ksd1, interrupt_T);
-int ctrl_switch = 15;
+float Ksp = 1.5; // Steering proportion
+float Ksi = 9000000; // Steering integral
+float Ksd = 1.2; // Steering derivative
+PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T); // Servo controller
Servo servo(PTA12); // Signal pin (PWM)
// ======
// Camera
// ======
-int t_int = 10000; // Exposure time
-bool rdyFlag = 1; // Signal when camera is ready again
-bool dataFlag = 0; // Signal when data is ready again
-DigitalOut debug(LED_BLUE);
-Timeout debugger;
-int fired = 0;
-
+int fixed_t_int = 1100; // Fixed exposure time
+int t_int = 10000; // Variable exposure time
+bool rdyFlag = 1; // Camera ready
+bool dataFlag = 0; // Data ready
const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
const int T_INT_MIN = 35; // Minimum exposure time
int img[128] = {0}; // Image data
@@ -140,289 +105,188 @@
// Functions
// ================
-
-//void killswitch(MODSERIAL_IRQ_INFO *q) {
-// MODSERIAL *serial = q->serial;
-// if (serial->rxGetLastChar() == 'k') {
-// e_stop = true;
-// motor = 1.0;
-// }
-// if (serial->rxGetLastChar() == '+') {
-// ref_v = ref_v + 0.05;
-// motor_ctrl.setSetPoint(ref_v);
-// }
-// if (serial->rxGetLastChar() == '-') {
-// ref_v = ref_v - 0.05;
-// motor_ctrl.setSetPoint(ref_v);
-// }
-//}
+void rxCallback(MODSERIAL_IRQ_INFO *q) {
+ MODSERIAL *serial = q->serial;
+ if ( serial->rxGetLastChar() == '+') {
+ master_v = master_v + 0.25;
+ motor_ctrl.setSetPoint(master_v);
+ }
+ if ( serial->rxGetLastChar() == '-') {
+ master_v = master_v - 0.25;
+ motor_ctrl.setSetPoint(master_v);
+ }
+}
// Communications
-//void communication(void const *args) {
-// telemetry_serial.baud(115200);
-// telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
-// telemetry_obj.transmit_header();
-// while (true) {
-// tele_time_ms = t_tele.read_ms();
-// telemetry_obj.do_io();
-// }
-//}
-
-int fixed_t_int = 1100;
-
void communication(void const *args) {
- // Initialize bluetooth
- bt.baud(115200);
-
+ telemetry_serial.baud(115200);
+ telemetry_serial.attach(&rxCallback, MODSERIAL::RxIrq);
+ telemetry_obj.transmit_header();
while (true) {
+ tele_time_ms = t_tele.read_ms();
+ telemetry_obj.do_io();
+ }
+}
- bt.printf("\r\nPress q to return to this prompt.\r\n");
- bt.printf("Available diagnostics:\r\n");
-// bt.printf(" [0] Velocity\r\n");
- bt.printf(" [0] Change exposure (us)\r\n");
- bt.printf(" [1] Steering\r\n");
-// bt.printf(" [2] Change Kmp\r\n");
-// bt.printf(" [3] Change Kmi\r\n");
-// bt.printf(" [4] Change Kmd\r\n");
- bt.printf(" [2] Change Ksp\r\n");
- bt.printf(" [3] Change Ksi\r\n");
- bt.printf(" [4] Change Ksd\r\n");
-// bt.printf(" [5] Change Ksp1\r\n");
- bt.printf(" [5] Change turn slowing minimum velocity\r\n");
-// bt.printf(" [6] Change reference velocity\r\n");
- bt.printf(" [6] Change turn slowing gain\r\n");
-// bt.printf(" [7] EMERGENCY STOP\r\n");
- bt.printf(" [7] Change turn slowing dead zone\r\n");
-// bt.printf(" [8] Timing\r\n");
- bt.printf(" [8] master_v += 0.05\r\n");
- bt.printf(" [9] master_v -= 0.05\r\n");
- comm_cmd = bt.getc();
- while (comm_cmd != 'q') {
- t.reset();
- t.start();
- switch(atoi(&comm_cmd)){
- case 0:
-// 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);
- bt.printf("Current: %d, New (8 digits): ", fixed_t_int);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- fixed_t_int = atoi(comm_in);
- comm_cmd = 'q';
- break;
- case 1:
- bt.printf("max: %d, Servo angle: %f, Track center: %d, t_int: %d,rdyFlag: %d, dataFlag: %d, fired: %d, timer: %d\r\n", max, angle, center, t_int,rdyFlag,dataFlag,fired,t_coms);
- break;
- case 2:
- bt.printf("Current: %f, New (8 digits): ", Ksp);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- Ksp = atof(comm_in);
- servo_ctrl.setTunings(Ksp, Ksi, Ksd);
- servo_ctrl.reset();
- comm_cmd = 'q';
- break;
- case 3:
- bt.printf("Current: %f, New (8 digits): ", Ksi);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- Ksi = atof(comm_in);
- servo_ctrl.setTunings(Ksp, Ksi, Ksd);
- servo_ctrl.reset();
- comm_cmd = 'q';
- break;
- case 4:
- bt.printf("Current: %f, New (8 digits): ", Ksd);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- Ksd = atof(comm_in);
- servo_ctrl.setTunings(Ksp, Ksi, Ksd);
- servo_ctrl.reset();
- comm_cmd = 'q';
- break;
-// case 2:
-// bt.printf("Current: %f, New (8 digits): ", Kmp);
+//void communication(void const *args) {
+// // Initialize bluetooth
+// bt.baud(115200);
+//
+// while (true) {
+//
+// bt.printf("\r\nPress q to return to this prompt.\r\n");
+// bt.printf("Available diagnostics:\r\n");
+//// bt.printf(" [0] Velocity\r\n");
+// bt.printf(" [0] Change exposure (us)\r\n");
+// bt.printf(" [1] Steering\r\n");
+// bt.printf(" [2] Change Ksp\r\n");
+// bt.printf(" [3] Change Ksi\r\n");
+// bt.printf(" [4] Change Ksd\r\n");
+// bt.printf(" [5] Change turn slowing minimum velocity\r\n");
+// bt.printf(" [6] Change turn slowing gain\r\n");
+// bt.printf(" [7] Change turn slowing dead zone\r\n");
+// bt.printf(" [8] master_v += 0.05\r\n");
+// bt.printf(" [9] master_v -= 0.05\r\n");
+// comm_cmd = bt.getc();
+// while (comm_cmd != 'q') {
+// t.reset();
+// t.start();
+// switch(atoi(&comm_cmd)){
+// case 0:
+//// 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);
+// bt.printf("Current: %d, New (8 digits): ", fixed_t_int);
// for (int i = 0; i < 8; i++) {
// comm_in[i] = bt.getc();
// bt.putc(comm_in[i]);
// }
// bt.printf("\r\n");
-// Kmp = atof(comm_in);
-// motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+// fixed_t_int = atoi(comm_in);
// comm_cmd = 'q';
// break;
-// case 3:
-// bt.printf("Current: %f, New (8 digits): ", Kmi);
+// case 1:
+// bt.printf("max: %d, angle: %f, center: %d, t_int: %d, rdyFlag: %d, dataFlag: %d\r\n", max, angle, center, t_int, rdyFlag, dataFlag);
+// break;
+// case 2:
+// bt.printf("Current: %f, New (8 digits): ", Ksp);
// for (int i = 0; i < 8; i++) {
// comm_in[i] = bt.getc();
// bt.putc(comm_in[i]);
// }
// bt.printf("\r\n");
-// Kmi = atof(comm_in);
-// motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+// Ksp = atof(comm_in);
+// servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+// servo_ctrl.reset();
// comm_cmd = 'q';
// break;
-// case 4:
-// bt.printf("Current: %f, New (8 digits): ", Kmd);
+// case 3:
+// bt.printf("Current: %f, New (8 digits): ", Ksi);
// for (int i = 0; i < 8; i++) {
// comm_in[i] = bt.getc();
// bt.putc(comm_in[i]);
// }
// bt.printf("\r\n");
-// Kmd = atof(comm_in);
-// motor_ctrl.setTunings(Kmp, Kmi, Kmd);
+// Ksi = atof(comm_in);
+// servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+// servo_ctrl.reset();
// comm_cmd = 'q';
// break;
-// case 5:
-// bt.printf("Current: %f, New (8 digits): ", Ksp1);
+// case 4:
+// bt.printf("Current: %f, New (8 digits): ", Ksd);
// for (int i = 0; i < 8; i++) {
// comm_in[i] = bt.getc();
// bt.putc(comm_in[i]);
// }
// bt.printf("\r\n");
-// Ksp1 = atof(comm_in);
-// servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
+// Ksd = atof(comm_in);
+// servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+// servo_ctrl.reset();
// comm_cmd = 'q';
// break;
- case 5:
- bt.printf("Current: %f, New (8 digits): ", turn_minv);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- turn_minv = atof(comm_in);
- comm_cmd = 'q';
- break;
- case 6:
- bt.printf("Current: %f, New (8 digits): ", turn_gain);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- turn_gain = atof(comm_in);
- comm_cmd = 'q';
- break;
+// case 5:
+// bt.printf("Current: %f, New (8 digits): ", turn_minv);
+// for (int i = 0; i < 8; i++) {
+// comm_in[i] = bt.getc();
+// bt.putc(comm_in[i]);
+// }
+// bt.printf("\r\n");
+// turn_minv = atof(comm_in);
+// comm_cmd = 'q';
+// break;
// case 6:
-// bt.printf("Current: %f, New (8 digits): ", ref_v);
+// bt.printf("Current: %f, New (8 digits): ", turn_gain);
+// for (int i = 0; i < 8; i++) {
+// comm_in[i] = bt.getc();
+// bt.putc(comm_in[i]);
+// }
+// bt.printf("\r\n");
+// turn_gain = atof(comm_in);
+// comm_cmd = 'q';
+// break;
+// case 7:
+// bt.printf("Current: %d, New (8 digits): ", turn_thresh);
// for (int i = 0; i < 8; i++) {
// comm_in[i] = bt.getc();
// bt.putc(comm_in[i]);
// }
// bt.printf("\r\n");
-// ref_v = atof(comm_in);
-// motor_ctrl.setSetPoint(ref_v);
+// turn_thresh = atoi(comm_in);
// comm_cmd = 'q';
// break;
-// case 7:
-// e_stop = true;
-// motor = 1.0;
-// bt.printf("STOPPED\r\n");
+// case 8:
+// master_v = master_v + 0.25;
+// motor_ctrl.setSetPoint(master_v);
+// bt.printf("%f\r\n", master_v);
// comm_cmd = 'q';
// break;
- case 7:
- bt.printf("Current: %d, New (8 digits): ", turn_thresh);
- for (int i = 0; i < 8; i++) {
- comm_in[i] = bt.getc();
- bt.putc(comm_in[i]);
- }
- bt.printf("\r\n");
- turn_thresh = atoi(comm_in);
- comm_cmd = 'q';
- break;
-// case 8:
-// 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);
+// case 9:
+// master_v = master_v - 0.25;
+// motor_ctrl.setSetPoint(master_v);
+// bt.printf("%f\r\n", master_v);
+// comm_cmd = 'q';
// break;
- case 8:
- master_v = master_v + 0.25;
- motor_ctrl.setSetPoint(master_v);
-// motor_duty = motor_duty + 0.05;
-// ref_pwm = ref_pwm + 0.05;
-// motor = 1.0 - motor_duty;
-// bt.printf("%f\r\n", motor_duty);
-// bt.printf("%f\r\n", ref_pwm);
- bt.printf("%f\r\n", master_v);
- comm_cmd = 'q';
- break;
- case 9:
- master_v = master_v - 0.25;
- motor_ctrl.setSetPoint(master_v);
-// motor_duty = motor_duty - 0.05;
-// ref_pwm = ref_pwm - 0.05;
-// motor = 1.0 - motor_duty;
-// bt.printf("%f\r\n", motor_duty);
-// bt.printf("%f\r\n", ref_pwm);
- bt.printf("%f\r\n", master_v);
- comm_cmd = 'q';
- break;
- }
- if (bt.readable()) {
- comm_cmd = bt.getc();
- }
-// t_coms = t.read_us();
- }
-
- }
-}
-
-//void bugger(){
-// debug = !debug;
+// }
+// if (bt.readable()) {
+// comm_cmd = bt.getc();
+// }
+// }
+//
+// }
//}
void cam_Read(){
- PTD->PCOR = (0x01);
- PTD->PSOR = (0x01 << 5);
- PTD->PSOR = (0x01);
- PTD->PCOR = (0x01 << 5);
+ // Image capture
+ clr_clk();
+ set_si();
+ set_clk();
+ clr_si();
for (int i = 0; i < 128; i++) {
- PTD->PCOR = (0x01);
+ clr_clk();
img[i] = ao.read_u16();
-// tele_linescan[i] = img[i];
- PTD->PSOR = (0x01);
+ tele_linescan[i] = img[i];
+ set_clk();
}
dataFlag = 1;
-// debugger.attach(bugger,0.2);
-// fired ++;
}
void control() {
- // Image capture
- //t.reset();
- //t.start();
-// DigitalOut debug2(LED_GREEN);
-// debug2 = 1;
- // Dummy read
if (rdyFlag){
- PTD->PCOR = (0x01);
- PTD->PSOR = (0x01 << 5);
- PTD->PSOR = (0x01);
- PTD->PCOR = (0x01 << 5);
+ // Dummy read
+ clr_clk();
+ set_si();
+ set_clk();
+ clr_clk();
for (int i = 0; i < 128; i++) {
- PTD->PCOR = (0x01);
- PTD->PSOR = (0x01);
+ clr_clk();
+ set_clk();
}
// Expose
expo_time.attach_us(&cam_Read,t_int);
rdyFlag = 0;
-
}
-
// Detect peak edges
if(dataFlag){
j = 0;
@@ -456,27 +320,10 @@
}
prev_center = center;
center = centers[best_score_idx];
-// tele_center = center;
+ tele_center = center;
- // Set servo angle
- //angle = 88 + (55 - center) * Ksp;
- // if (angle > 113) {
- // angle = 113;
- // }
- // if (angle < 63) {
- // angle = 63;
- // }
- // servo = angle / 180;
-
- // if (abs(center - 64) > ctrl_switch) {
-// servo_ctrl1.setProcessValue(center);
-// angle = 88+servo_ctrl1.compute();
-// servo_ctrl.reset();
-// } else {
- servo_ctrl.setProcessValue(center);
- angle = 88 + servo_ctrl.compute();
-// servo_ctrl1.reset();
- // }
+ servo_ctrl.setProcessValue(center);
+ angle = 88 + servo_ctrl.compute();
servo = angle / 180;
@@ -500,28 +347,25 @@
t_int = T_INT_MAX;
}
t_int = fixed_t_int;
-// tele_exposure = t_int;
servo_ctrl.setInterval(t_int);
rdyFlag = 1;
dataFlag = 0;
- //t_main = t.read_us();
}
+ // Velocity control
if (motor_ctrl_count == 1000) {
velocity = (40000 - bemf_vel) / 6000.0;
-// motor_duty = motor_duty + 0.1;
-// motor = 1.0 - motor_duty;
+ tele_velocity = velocity;
motor_ctrl_count = 0;
motor_ctrl.setProcessValue(velocity);
motor_duty = motor_ctrl.compute();
motor = motor_duty;
motor_ctrl_count = 0;
-// }
-// motor_ctrl_count = motor_ctrl_count + 1;
} else {
motor_ctrl_count = motor_ctrl_count + 1;
}
+ // Turn handling
ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
if (ref_v < turn_minv) {
ref_v = turn_minv;
@@ -532,70 +376,14 @@
if (abs(motor_ctrl.getSetPoint() - ref_v) > 0.05) {
motor_ctrl.setSetPoint(ref_v);
}
-
-// motor_duty = ref_pwm - ref_pwm * turn_gain * (abs(64 - center) - turn_thresh);
-// if (motor_duty > ref_pwm) {
-// motor_duty = ref_pwm;
-// }
-// if (motor_duty < turn_minpwm) {
-// motor_duty = turn_minpwm;
-// }
-// if (abs(1.0 - motor.read() - motor_duty) > 0.01) {
-// motor = 1.0 - motor_duty;
-// }
-
- // t_steer = t.read_us();
-
-
-
-// // Velocity control
-// // t.reset();
-// if (!e_stop) {
-// curr_pulses = qei.getPulses();
-// //for (int i = 0; i < 9; i++) {
-//// prev_vels[i] = prev_vels[i+1];
-//// }
-//// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
-//// t.reset();
-//// if (prev_vels[9] < 0) {
-//// prev_vels[9] = 0;
-//// }
-//// if (prev_vels[0] < 0) {
-//// velocity = prev_vels[9];
-//// } else {
-//// velocity = 0;
-//// for (int i = 0; i < 10; i++) {
-//// velocity = velocity + prev_vels[i];
-//// velocity = velocity / 10;
-//// }
-//// }
-// velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
-// if (velocity < 0) {
-// velocity = 0;
-// }
-//// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
-// t.reset();
-// tele_velocity = velocity;
-// prev_pulses = curr_pulses;
-// motor_ctrl.setProcessValue(velocity);
-// motor_duty = motor_ctrl.compute();
-// motor = 1.0 - motor_duty;
-// tele_pwm = motor_duty;
-// } else {
-// motor = 1.0;
-// }
-// // t_vel = t.read_us();
-// ctrl_flag = false;
}
-void set_control_flag() {
- ctrl_flag = true;
-}
-
+// Back emf measuring interrupt
void meas_bemf() {
bemf_vel = bemf.read_u16();
}
+// Back emf trigger
void bemf_irq() {
bemf_timeout.attach(&meas_bemf, 0.002);
}
@@ -604,16 +392,7 @@
// Main
// ====
int main() {
- // t.start();
-// t_tele.start();
-// tele_center.set_limits(0, 128);
-// tele_pwm.set_limits(0.0, 1.0);
-
- bemf_int.fall(&bemf_irq);
-
-// for (int i = 0; i < 10; i++) {
-// prev_vels[i] = -1;
-// }
+ t_tele.start();
// Initialize motor
motor.period(motor_T);
@@ -626,30 +405,25 @@
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);
+ // Initialize back EMF reader
+ bemf_int.fall(&bemf_irq);
+
// Initialize servo
servo.calibrate(0.001, 45.0);
servo = angle / 180.0;
+ // Initialize servo controller
servo_ctrl.setInputLimits(0, 127);
- //servo_ctrl.setOutputLimits(-25, 25);
servo_ctrl.setOutputLimits(-30, 30);
servo_ctrl.setSetPoint(64);
servo_ctrl.setBias(0.0);
servo_ctrl.setMode(1);
-// servo_ctrl1.setInputLimits(10, 107);
-// servo_ctrl1.setOutputLimits(-30, 30);
-// servo_ctrl1.setSetPoint(64);
-// servo_ctrl1.setBias(0.0);
-// servo_ctrl1.setMode(1);
-
// Initialize communications thread
Thread communication_thread(communication);
-
-// control_interrupt.attach(control, interrupt_T);
-// control_interrupt.attach(set_control_flag, interrupt_T);
+ // Run
while (true) {
- control();
- }
+ control();
+ }
}
\ No newline at end of file