Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

callbacks.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
251:4ba2f238066f 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

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