N K / Mbed 2 deprecated analoghalls_part_4

yup

Dependencies:   mbed

Fork of analoghalls by Bayley Wang

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?

UserRevisionLine numberNew 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 }