N K
/
analoghalls_part_4
yup
Fork of analoghalls by
util.cpp@3:86ccde39f61b, 2015-02-25 (annotated)
- Committer:
- bwang
- Date:
- Wed Feb 25 04:44:05 2015 +0000
- Revision:
- 3:86ccde39f61b
- Parent:
- 2:b5c19d4eddcc
- Child:
- 5:eeb8af99cb6c
such debug wow
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nki | 0:9753f3c2e5ca | 1 | #include "constants.h" |
nki | 0:9753f3c2e5ca | 2 | #include "shared.h" |
nki | 0:9753f3c2e5ca | 3 | #include "isr.h" |
nki | 0:9753f3c2e5ca | 4 | #include <stdlib.h> |
nki | 0:9753f3c2e5ca | 5 | |
nki | 0:9753f3c2e5ca | 6 | void initTimers() { |
bwang | 2:b5c19d4eddcc | 7 | #ifdef __USE_THROTTLE |
bwang | 1:70eed554399b | 8 | dtc_upd_ticker.attach_us(&dtc_update, 200); |
bwang | 1:70eed554399b | 9 | throttle_upd_ticker.attach_us(&throttle_update, 100000); |
bwang | 2:b5c19d4eddcc | 10 | #endif |
bwang | 2:b5c19d4eddcc | 11 | |
bwang | 2:b5c19d4eddcc | 12 | #ifndef __NATIVE |
bwang | 2:b5c19d4eddcc | 13 | pha.period_us(100); |
bwang | 2:b5c19d4eddcc | 14 | phb.period_us(100); |
bwang | 2:b5c19d4eddcc | 15 | phc.period_us(100); |
nki | 0:9753f3c2e5ca | 16 | pha = 0; |
nki | 0:9753f3c2e5ca | 17 | phb = 1.0f; |
nki | 0:9753f3c2e5ca | 18 | phc = 0; |
bwang | 2:b5c19d4eddcc | 19 | #endif |
nki | 0:9753f3c2e5ca | 20 | |
bwang | 2:b5c19d4eddcc | 21 | TIM2->CR1 &= ~(TIM_CR1_CEN); |
bwang | 2:b5c19d4eddcc | 22 | TIM2->CR1 |= TIM_CR1_CMS; |
bwang | 2:b5c19d4eddcc | 23 | |
bwang | 2:b5c19d4eddcc | 24 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 25 | TIM2->CCMR1 |= TIM_CCMR1_OC1M_2; |
bwang | 2:b5c19d4eddcc | 26 | TIM2->CCMR1 |= TIM_CCMR1_OC1M_1; |
bwang | 2:b5c19d4eddcc | 27 | TIM2->CCMR1 |= TIM_CCMR1_OC2M_2; |
bwang | 2:b5c19d4eddcc | 28 | TIM2->CCMR1 |= TIM_CCMR1_OC2M_1; |
bwang | 2:b5c19d4eddcc | 29 | TIM2->CCMR2 |= TIM_CCMR2_OC3M_2; |
bwang | 2:b5c19d4eddcc | 30 | TIM2->CCMR2 |= TIM_CCMR2_OC3M_1; |
bwang | 2:b5c19d4eddcc | 31 | |
bwang | 2:b5c19d4eddcc | 32 | TIM2->CCER &= ~TIM_CCER_CC1P; |
bwang | 2:b5c19d4eddcc | 33 | TIM2->CCER &= ~TIM_CCER_CC2P; |
bwang | 2:b5c19d4eddcc | 34 | TIM2->CCER &= ~TIM_CCER_CC3P; |
bwang | 2:b5c19d4eddcc | 35 | |
bwang | 2:b5c19d4eddcc | 36 | TIM2->CCER |= TIM_CCER_CC1E; |
bwang | 2:b5c19d4eddcc | 37 | TIM2->CCER |= TIM_CCER_CC2E; |
bwang | 2:b5c19d4eddcc | 38 | TIM2->CCER |= TIM_CCER_CC3E; |
bwang | 2:b5c19d4eddcc | 39 | |
bwang | 2:b5c19d4eddcc | 40 | TIM2->PSC = 0; |
bwang | 2:b5c19d4eddcc | 41 | |
bwang | 2:b5c19d4eddcc | 42 | TIM2->ARR = 10000; |
bwang | 2:b5c19d4eddcc | 43 | TIM2->CCR1 = 0; |
bwang | 2:b5c19d4eddcc | 44 | TIM2->CCR2 = 0; |
bwang | 2:b5c19d4eddcc | 45 | TIM2->CCR3 = 0; |
bwang | 2:b5c19d4eddcc | 46 | #endif |
bwang | 2:b5c19d4eddcc | 47 | |
bwang | 2:b5c19d4eddcc | 48 | TIM2->CR1 |= TIM_CR1_CEN; |
nki | 0:9753f3c2e5ca | 49 | } |
nki | 0:9753f3c2e5ca | 50 | |
bwang | 1:70eed554399b | 51 | void initPins() { |
nki | 0:9753f3c2e5ca | 52 | en = 0; |
nki | 0:9753f3c2e5ca | 53 | } |
nki | 0:9753f3c2e5ca | 54 | |
nki | 0:9753f3c2e5ca | 55 | void initData() { |
nki | 0:9753f3c2e5ca | 56 | motor = (Motor*) malloc(sizeof(Motor)); |
nki | 0:9753f3c2e5ca | 57 | motor->sensor_phase = SENSOR_PHASE; |
bwang | 2:b5c19d4eddcc | 58 | motor->halt = 0; |
bwang | 2:b5c19d4eddcc | 59 | motor->angle = 0; |
bwang | 3:86ccde39f61b | 60 | motor->debug_stop = 0; |
bwang | 1:70eed554399b | 61 | |
bwang | 2:b5c19d4eddcc | 62 | #ifdef __USE_THROTTLE |
nki | 0:9753f3c2e5ca | 63 | float throttle_raw; |
nki | 0:9753f3c2e5ca | 64 | do { |
nki | 0:9753f3c2e5ca | 65 | throttle_raw = throttle; |
nki | 0:9753f3c2e5ca | 66 | throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB); |
nki | 0:9753f3c2e5ca | 67 | if (throttle_raw < 0.0f) throttle_raw = 0.0f; |
nki | 0:9753f3c2e5ca | 68 | } while (throttle_raw > 0.05f); |
nki | 0:9753f3c2e5ca | 69 | |
nki | 0:9753f3c2e5ca | 70 | motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle; |
nki | 0:9753f3c2e5ca | 71 | if (motor->throttle < 0.05f) { |
nki | 0:9753f3c2e5ca | 72 | motor->halt = 1; |
nki | 0:9753f3c2e5ca | 73 | } else { |
nki | 0:9753f3c2e5ca | 74 | motor->halt = 0; |
nki | 0:9753f3c2e5ca | 75 | } |
bwang | 2:b5c19d4eddcc | 76 | #else |
bwang | 2:b5c19d4eddcc | 77 | motor->throttle = 1.0f; |
bwang | 2:b5c19d4eddcc | 78 | #endif |
bwang | 2:b5c19d4eddcc | 79 | } |
bwang | 2:b5c19d4eddcc | 80 | |
bwang | 2:b5c19d4eddcc | 81 | void setDtcA(float f) { |
bwang | 2:b5c19d4eddcc | 82 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 83 | TIM2->CCR3 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 84 | #else |
bwang | 2:b5c19d4eddcc | 85 | pha = f; |
bwang | 2:b5c19d4eddcc | 86 | #endif |
bwang | 2:b5c19d4eddcc | 87 | } |
bwang | 2:b5c19d4eddcc | 88 | |
bwang | 2:b5c19d4eddcc | 89 | void setDtcB(float f) { |
bwang | 2:b5c19d4eddcc | 90 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 91 | TIM2->CCR1 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 92 | #else |
bwang | 2:b5c19d4eddcc | 93 | phb = f; |
bwang | 2:b5c19d4eddcc | 94 | #endif |
bwang | 2:b5c19d4eddcc | 95 | } |
bwang | 2:b5c19d4eddcc | 96 | |
bwang | 2:b5c19d4eddcc | 97 | void setDtcC(float f) { |
bwang | 2:b5c19d4eddcc | 98 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 99 | TIM2->CCR2 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 100 | #else |
bwang | 2:b5c19d4eddcc | 101 | phc = f; |
bwang | 2:b5c19d4eddcc | 102 | #endif |
nki | 0:9753f3c2e5ca | 103 | } |