Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

callbacks.cpp@214:c70a6e86417f, 2018-04-29 (annotated)

Committer:
bwang
Date:
Sun Apr 29 05:02:14 2018 +0000
Revision:
214:c70a6e86417f
Parent:
212:1e370ffcb73d
Child:
246:167b5d50d0f2
added bounds check in rxCallback()

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 187:523cf8c962e4 1 #include "CommandProcessor.h"
bwang 187:523cf8c962e4 2
bwang 187:523cf8c962e4 3 #include "defaults.h"
bwang 191:66861311bdcd 4 #include "derived.h"
bwang 187:523cf8c962e4 5 #include "globals.h"
bwang 187:523cf8c962e4 6 #include "main.h"
bwang 187:523cf8c962e4 7
bwang 187:523cf8c962e4 8 char linebuf[128];
bwang 187:523cf8c962e4 9 int index = 0;
bwang 187:523cf8c962e4 10
bwang 187:523cf8c962e4 11 void rxCallback() {
bwang 187:523cf8c962e4 12 while (io.pc->readable()) {
bwang 187:523cf8c962e4 13 char c = io.pc->getc();
bwang 212:1e370ffcb73d 14 if (c != 127 && c != 8 && c != '\r' && c != '\t') {
bwang 187:523cf8c962e4 15 linebuf[index] = c;
bwang 214:c70a6e86417f 16 if (index < 127) index++;
bwang 187:523cf8c962e4 17 io.pc->putc(c);
bwang 212:1e370ffcb73d 18 } else if (c == 127 || c == 8) {
bwang 187:523cf8c962e4 19 if (index > 0) {
bwang 187:523cf8c962e4 20 index--;
bwang 212:1e370ffcb73d 21 //BS (8) should delete previous char
bwang 212:1e370ffcb73d 22 io.pc->putc(127);
bwang 187:523cf8c962e4 23 }
bwang 187:523cf8c962e4 24 } else if (c == '\r') {
bwang 187:523cf8c962e4 25 linebuf[index] = 0;
bwang 187:523cf8c962e4 26 io.pc->putc(c);
bwang 187:523cf8c962e4 27 processCmd(io.pc, io.pref, linebuf);
bwang 187:523cf8c962e4 28 index = 0;
bwang 187:523cf8c962e4 29 io.pc->putc('>');
bwang 187:523cf8c962e4 30 }
bwang 187:523cf8c962e4 31 }
bwang 187:523cf8c962e4 32 }
bwang 187:523cf8c962e4 33
bwang 187:523cf8c962e4 34 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 187:523cf8c962e4 35 int start_state = io.throttle_in->state();
bwang 187:523cf8c962e4 36
bwang 187:523cf8c962e4 37 if (TIM1->SR & TIM_SR_UIF ) {
bwang 187:523cf8c962e4 38 ADC1->CR2 |= 0x40000000;
bwang 187:523cf8c962e4 39 volatile int delay;
bwang 187:523cf8c962e4 40 for (delay = 0; delay < 35; delay++);
bwang 187:523cf8c962e4 41
bwang 187:523cf8c962e4 42 read.adval1 = ADC1->DR;
bwang 187:523cf8c962e4 43 read.adval2 = ADC2->DR;
bwang 187:523cf8c962e4 44
bwang 187:523cf8c962e4 45 commutate();
bwang 187:523cf8c962e4 46 }
bwang 187:523cf8c962e4 47 TIM1->SR = 0x00;
bwang 187:523cf8c962e4 48 int end_state = io.throttle_in->state();
bwang 187:523cf8c962e4 49 if (start_state != end_state) io.throttle_in->block();
bwang 187:523cf8c962e4 50 }
bwang 187:523cf8c962e4 51
bwang 187:523cf8c962e4 52 void update_velocity() {
bwang 187:523cf8c962e4 53 read.last_p_mech = read.p_mech;
bwang 187:523cf8c962e4 54 read.p_mech = io.pos->GetMechPosition();
bwang 187:523cf8c962e4 55
bwang 187:523cf8c962e4 56 float dp_mech = read.p_mech - read.last_p_mech;
bwang 187:523cf8c962e4 57 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 187:523cf8c962e4 58 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 187:523cf8c962e4 59
bwang 187:523cf8c962e4 60 float w_raw = dp_mech * F_SW; //rad/s
bwang 187:523cf8c962e4 61
bwang 187:523cf8c962e4 62 read.w = control.velocity_filter->update(w_raw);
bwang 187:523cf8c962e4 63 }