Bayley Wang
/
foc-ed_in_the_bot_compact
robot
callbacks.cpp@250:ef028cbd0749, 2018-11-13 (annotated)
- Committer:
- bwang
- Date:
- Tue Nov 13 17:26:58 2018 +0000
- Revision:
- 250:ef028cbd0749
- Parent:
- 249:314c1b12d9c2
- Child:
- 251:4ba2f238066f
11/13/2018 12:26 - bugfix to rxCallback()
Who changed what in which revision?
User | Revision | Line number | New 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 | 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 | 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 | } |