N K
/
analoghalls_part_4
yup
Fork of analoghalls by
util.cpp@6:4960629abb90, 2015-02-26 (annotated)
- Committer:
- nki
- Date:
- Thu Feb 26 14:09:19 2015 +0000
- Revision:
- 6:4960629abb90
- Parent:
- 5:eeb8af99cb6c
LOLOL PROPROTIONAL CURENT CONTROL;
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); |
nki | 5:eeb8af99cb6c | 10 | isense_upd_ticker.attach_us(&isense_update,1000); |
bwang | 2:b5c19d4eddcc | 11 | #endif |
bwang | 2:b5c19d4eddcc | 12 | |
bwang | 2:b5c19d4eddcc | 13 | #ifndef __NATIVE |
bwang | 2:b5c19d4eddcc | 14 | pha.period_us(100); |
bwang | 2:b5c19d4eddcc | 15 | phb.period_us(100); |
bwang | 2:b5c19d4eddcc | 16 | phc.period_us(100); |
nki | 0:9753f3c2e5ca | 17 | pha = 0; |
nki | 0:9753f3c2e5ca | 18 | phb = 1.0f; |
nki | 0:9753f3c2e5ca | 19 | phc = 0; |
bwang | 2:b5c19d4eddcc | 20 | #endif |
nki | 0:9753f3c2e5ca | 21 | |
bwang | 2:b5c19d4eddcc | 22 | TIM2->CR1 &= ~(TIM_CR1_CEN); |
bwang | 2:b5c19d4eddcc | 23 | TIM2->CR1 |= TIM_CR1_CMS; |
bwang | 2:b5c19d4eddcc | 24 | |
bwang | 2:b5c19d4eddcc | 25 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 26 | TIM2->CCMR1 |= TIM_CCMR1_OC1M_2; |
bwang | 2:b5c19d4eddcc | 27 | TIM2->CCMR1 |= TIM_CCMR1_OC1M_1; |
bwang | 2:b5c19d4eddcc | 28 | TIM2->CCMR1 |= TIM_CCMR1_OC2M_2; |
bwang | 2:b5c19d4eddcc | 29 | TIM2->CCMR1 |= TIM_CCMR1_OC2M_1; |
bwang | 2:b5c19d4eddcc | 30 | TIM2->CCMR2 |= TIM_CCMR2_OC3M_2; |
bwang | 2:b5c19d4eddcc | 31 | TIM2->CCMR2 |= TIM_CCMR2_OC3M_1; |
bwang | 2:b5c19d4eddcc | 32 | |
bwang | 2:b5c19d4eddcc | 33 | TIM2->CCER &= ~TIM_CCER_CC1P; |
bwang | 2:b5c19d4eddcc | 34 | TIM2->CCER &= ~TIM_CCER_CC2P; |
bwang | 2:b5c19d4eddcc | 35 | TIM2->CCER &= ~TIM_CCER_CC3P; |
bwang | 2:b5c19d4eddcc | 36 | |
bwang | 2:b5c19d4eddcc | 37 | TIM2->CCER |= TIM_CCER_CC1E; |
bwang | 2:b5c19d4eddcc | 38 | TIM2->CCER |= TIM_CCER_CC2E; |
bwang | 2:b5c19d4eddcc | 39 | TIM2->CCER |= TIM_CCER_CC3E; |
bwang | 2:b5c19d4eddcc | 40 | |
bwang | 2:b5c19d4eddcc | 41 | TIM2->PSC = 0; |
bwang | 2:b5c19d4eddcc | 42 | |
bwang | 2:b5c19d4eddcc | 43 | TIM2->ARR = 10000; |
bwang | 2:b5c19d4eddcc | 44 | TIM2->CCR1 = 0; |
bwang | 2:b5c19d4eddcc | 45 | TIM2->CCR2 = 0; |
bwang | 2:b5c19d4eddcc | 46 | TIM2->CCR3 = 0; |
bwang | 2:b5c19d4eddcc | 47 | #endif |
bwang | 2:b5c19d4eddcc | 48 | |
bwang | 2:b5c19d4eddcc | 49 | TIM2->CR1 |= TIM_CR1_CEN; |
nki | 0:9753f3c2e5ca | 50 | } |
nki | 0:9753f3c2e5ca | 51 | |
bwang | 1:70eed554399b | 52 | void initPins() { |
nki | 0:9753f3c2e5ca | 53 | en = 0; |
nki | 0:9753f3c2e5ca | 54 | } |
nki | 0:9753f3c2e5ca | 55 | |
nki | 0:9753f3c2e5ca | 56 | void initData() { |
nki | 0:9753f3c2e5ca | 57 | motor = (Motor*) malloc(sizeof(Motor)); |
nki | 0:9753f3c2e5ca | 58 | motor->sensor_phase = SENSOR_PHASE; |
bwang | 2:b5c19d4eddcc | 59 | motor->halt = 0; |
bwang | 2:b5c19d4eddcc | 60 | motor->angle = 0; |
bwang | 3:86ccde39f61b | 61 | motor->debug_stop = 0; |
bwang | 1:70eed554399b | 62 | |
bwang | 2:b5c19d4eddcc | 63 | #ifdef __USE_THROTTLE |
nki | 0:9753f3c2e5ca | 64 | float throttle_raw; |
nki | 0:9753f3c2e5ca | 65 | do { |
nki | 0:9753f3c2e5ca | 66 | throttle_raw = throttle; |
nki | 0:9753f3c2e5ca | 67 | throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB); |
nki | 0:9753f3c2e5ca | 68 | if (throttle_raw < 0.0f) throttle_raw = 0.0f; |
nki | 0:9753f3c2e5ca | 69 | } while (throttle_raw > 0.05f); |
nki | 0:9753f3c2e5ca | 70 | |
nki | 0:9753f3c2e5ca | 71 | motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle; |
nki | 5:eeb8af99cb6c | 72 | if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) { |
nki | 0:9753f3c2e5ca | 73 | motor->halt = 1; |
nki | 0:9753f3c2e5ca | 74 | } else { |
nki | 0:9753f3c2e5ca | 75 | motor->halt = 0; |
nki | 0:9753f3c2e5ca | 76 | } |
bwang | 2:b5c19d4eddcc | 77 | #else |
bwang | 2:b5c19d4eddcc | 78 | motor->throttle = 1.0f; |
bwang | 2:b5c19d4eddcc | 79 | #endif |
bwang | 2:b5c19d4eddcc | 80 | } |
bwang | 2:b5c19d4eddcc | 81 | |
bwang | 2:b5c19d4eddcc | 82 | void setDtcA(float f) { |
bwang | 2:b5c19d4eddcc | 83 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 84 | TIM2->CCR3 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 85 | #else |
bwang | 2:b5c19d4eddcc | 86 | pha = f; |
bwang | 2:b5c19d4eddcc | 87 | #endif |
bwang | 2:b5c19d4eddcc | 88 | } |
bwang | 2:b5c19d4eddcc | 89 | |
bwang | 2:b5c19d4eddcc | 90 | void setDtcB(float f) { |
bwang | 2:b5c19d4eddcc | 91 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 92 | TIM2->CCR1 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 93 | #else |
bwang | 2:b5c19d4eddcc | 94 | phb = f; |
bwang | 2:b5c19d4eddcc | 95 | #endif |
bwang | 2:b5c19d4eddcc | 96 | } |
bwang | 2:b5c19d4eddcc | 97 | |
bwang | 2:b5c19d4eddcc | 98 | void setDtcC(float f) { |
bwang | 2:b5c19d4eddcc | 99 | #ifdef __NATIVE |
bwang | 2:b5c19d4eddcc | 100 | TIM2->CCR2 = (uint16_t) (f * 10000.0f); |
bwang | 2:b5c19d4eddcc | 101 | #else |
bwang | 2:b5c19d4eddcc | 102 | phc = f; |
bwang | 2:b5c19d4eddcc | 103 | #endif |
nki | 0:9753f3c2e5ca | 104 | } |