N K
/
analoghalls_part_4
yup
Fork of analoghalls by
util.cpp@2:b5c19d4eddcc, 2015-02-23 (annotated)
- Committer:
- bwang
- Date:
- Mon Feb 23 19:42:50 2015 +0000
- Revision:
- 2:b5c19d4eddcc
- Parent:
- 1:70eed554399b
- Child:
- 3:86ccde39f61b
stuff
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 | 1:70eed554399b | 60 | |
bwang | 2:b5c19d4eddcc | 61 | #ifdef __USE_THROTTLE |
nki | 0:9753f3c2e5ca | 62 | float throttle_raw; |
nki | 0:9753f3c2e5ca | 63 | do { |
nki | 0:9753f3c2e5ca | 64 | throttle_raw = throttle; |
nki | 0:9753f3c2e5ca | 65 | throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB); |
nki | 0:9753f3c2e5ca | 66 | if (throttle_raw < 0.0f) throttle_raw = 0.0f; |
nki | 0:9753f3c2e5ca | 67 | } while (throttle_raw > 0.05f); |
nki | 0:9753f3c2e5ca | 68 | |
nki | 0:9753f3c2e5ca | 69 | motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle; |
nki | 0:9753f3c2e5ca | 70 | if (motor->throttle < 0.05f) { |
nki | 0:9753f3c2e5ca | 71 | motor->halt = 1; |
nki | 0:9753f3c2e5ca | 72 | } else { |
nki | 0:9753f3c2e5ca | 73 | motor->halt = 0; |
nki | 0:9753f3c2e5ca | 74 | } |
bwang | 2:b5c19d4eddcc | 75 | #else |
bwang | 2:b5c19d4eddcc | 76 | motor->throttle = 1.0f; |
bwang | 2:b5c19d4eddcc | 77 | #endif |
bwang | 2:b5c19d4eddcc | 78 | } |
bwang | 2:b5c19d4eddcc | 79 | |
bwang | 2:b5c19d4eddcc | 80 | void setDtcA(float f) { |
bwang | 2:b5c19d4eddcc | 81 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 82 | TIM2->CCR3 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 83 | #else |
bwang | 2:b5c19d4eddcc | 84 | pha = f; |
bwang | 2:b5c19d4eddcc | 85 | #endif |
bwang | 2:b5c19d4eddcc | 86 | } |
bwang | 2:b5c19d4eddcc | 87 | |
bwang | 2:b5c19d4eddcc | 88 | void setDtcB(float f) { |
bwang | 2:b5c19d4eddcc | 89 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 90 | TIM2->CCR1 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 91 | #else |
bwang | 2:b5c19d4eddcc | 92 | phb = f; |
bwang | 2:b5c19d4eddcc | 93 | #endif |
bwang | 2:b5c19d4eddcc | 94 | } |
bwang | 2:b5c19d4eddcc | 95 | |
bwang | 2:b5c19d4eddcc | 96 | void setDtcC(float f) { |
bwang | 2:b5c19d4eddcc | 97 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 98 | TIM2->CCR2 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 99 | #else |
bwang | 2:b5c19d4eddcc | 100 | phc = f; |
bwang | 2:b5c19d4eddcc | 101 | #endif |
nki | 0:9753f3c2e5ca | 102 | } |