Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

callbacks.cpp@249:314c1b12d9c2, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:23:50 2018 +0000
Revision:
249:314c1b12d9c2
Parent:
247:da647f7185b7
Child:
250:ef028cbd0749
11/13/2018 12:33 - changes to rxCallback() - fast commands no longer print "\r>" after completion; non-ASCII chars are no longer echoed

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 249:314c1b12d9c2 17 if (c < 128) 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 249:314c1b12d9c2 26 if (!io.cmd_busy && linebuf[0] > 127) {
bwang 249:314c1b12d9c2 27 processCmdFast(io.pc, io.pref, linebuf);
bwang 249:314c1b12d9c2 28 } else if (!io.cmd_busy) {
bwang 249:314c1b12d9c2 29 processCmd(io.pc, io.pref, linebuf);
bwang 249:314c1b12d9c2 30 io.pc->putc(c);
bwang 249:314c1b12d9c2 31 io.pc->putc('>');
bwang 247:da647f7185b7 32 }
bwang 187:523cf8c962e4 33 index = 0;
bwang 187:523cf8c962e4 34 }
bwang 187:523cf8c962e4 35 }
bwang 187:523cf8c962e4 36 }
bwang 187:523cf8c962e4 37
bwang 187:523cf8c962e4 38 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 187:523cf8c962e4 39 int start_state = io.throttle_in->state();
bwang 187:523cf8c962e4 40
bwang 187:523cf8c962e4 41 if (TIM1->SR & TIM_SR_UIF ) {
bwang 187:523cf8c962e4 42 ADC1->CR2 |= 0x40000000;
bwang 187:523cf8c962e4 43 volatile int delay;
bwang 187:523cf8c962e4 44 for (delay = 0; delay < 35; delay++);
bwang 187:523cf8c962e4 45
bwang 187:523cf8c962e4 46 read.adval1 = ADC1->DR;
bwang 187:523cf8c962e4 47 read.adval2 = ADC2->DR;
bwang 187:523cf8c962e4 48
bwang 187:523cf8c962e4 49 commutate();
bwang 187:523cf8c962e4 50 }
bwang 187:523cf8c962e4 51 TIM1->SR = 0x00;
bwang 187:523cf8c962e4 52 int end_state = io.throttle_in->state();
bwang 187:523cf8c962e4 53 if (start_state != end_state) io.throttle_in->block();
bwang 187:523cf8c962e4 54 }
bwang 187:523cf8c962e4 55
bwang 187:523cf8c962e4 56 void update_velocity() {
bwang 187:523cf8c962e4 57 read.last_p_mech = read.p_mech;
bwang 187:523cf8c962e4 58 read.p_mech = io.pos->GetMechPosition();
bwang 187:523cf8c962e4 59
bwang 187:523cf8c962e4 60 float dp_mech = read.p_mech - read.last_p_mech;
bwang 187:523cf8c962e4 61 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 187:523cf8c962e4 62 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 187:523cf8c962e4 63
bwang 187:523cf8c962e4 64 float w_raw = dp_mech * F_SW; //rad/s
bwang 187:523cf8c962e4 65
bwang 187:523cf8c962e4 66 read.w = control.velocity_filter->update(w_raw);
bwang 187:523cf8c962e4 67 }