util.cpp

00001 #include "constants.h"
00002 #include "shared.h"
00003 #include "isr.h"
00004 #include <stdlib.h>
00005 
00006 void initTimers() {
00007     dtc_upd_ticker.attach_us(&dtc_update, 20);    
00008     pha.period_us(200);
00009     phb.period_us(200);
00010     phc.period_us(200);
00011     pha = 0;
00012     phb = 1.0f;
00013     phc = 0;
00014 
00015 }
00016 
00017 void initPins() {    
00018     halla.mode(PullUp);
00019     hallb.mode(PullUp);
00020     hallc.mode(PullUp);
00021     
00022     en = 0;
00023 }
00024 
00025 void initData() {
00026     motor = (Motor*) malloc(sizeof(Motor));
00027     motor->hall = (halla.read() << 2) + (hallb.read() << 1) + hallc.read();
00028     motor->last_hall = 0;
00029     motor->reverses = 0;
00030     motor->last_time = 3000.0f;
00031     motor->major_pos = 60.0f * ATable[motor->hall];
00032     motor->ticks = 0.0f;
00033     motor->sensor_phase = SENSOR_PHASE;
00034     motor->halt = 1;
00035     motor->whangle =0;
00036     motor->analoga = analoga;
00037     motor->analogb = analogb;
00038     
00039     float throttle_raw;
00040     do {
00041         throttle_raw = throttle;
00042         throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
00043         if (throttle_raw < 0.0f) throttle_raw = 0.0f;
00044     } while (throttle_raw > 0.05f);
00045     
00046     motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
00047     if (motor->throttle < 0.05f) {
00048         motor->halt = 1;
00049     } else {
00050         motor->halt = 0;
00051     }
00052 }