Remi Beges
Fork of the official library for the FRDM-TFC shield to add measurement of H-bridge sense currents
Dependents: telemetry_demo_FRDM-TFC
Fork of
FRDM-TFC
by 
Eli Hughes
Embed:
(wiki syntax)
Show/hide line numbers
TFC.cpp
00001 #include "mbed.h" 00002 #include "TFC.h" 00003 00004 #define FTM1_CLK_PRESCALE 6// Prescale Selector value - see comments in Status Control (SC) section for more details 00005 #define SERVO_DEFAULT_PERIOD (float)(.020) // Desired Frequency of PWM Signal - Here 50Hz => 20ms period 00006 // use these to dial in servo steering to your particular servo 00007 #define SERVO_MIN_PULSE_WIDTH_DEFAULT (float)(.00095) // The number here should be be *pulse width* in seconds to move servo to its left limit 00008 #define SERVO_MAX_PULSE_WIDTH_DEFAULT (float)(.00160) // The number here should be be *pulse width* in seconds to move servo to its left limit 00009 00010 00011 #define FTM0_CLOCK (SystemCoreClock/2) 00012 #define FTM0_CLK_PRESCALE (0) // Prescale Selector value - see comments in Status Control (SC) section for more details 00013 #define FTM0_DEFAULT_SWITCHING_FREQUENCY (4000.0) 00014 00015 #define ADC_MAX_CODE (4095) 00016 00017 #define TAOS_CLK_HIGH PTE->PSOR = (1<<1) 00018 #define TAOS_CLK_LOW PTE->PCOR = (1<<1) 00019 #define TAOS_SI_HIGH PTD->PSOR = (1<<7) 00020 #define TAOS_SI_LOW PTD->PCOR = (1<<7) 00021 00022 #define ADC_STATE_INIT 0 00023 #define ADC_STATE_CAPTURE_POT_0 1 00024 #define ADC_STATE_CAPTURE_POT_1 2 00025 #define ADC_STATE_CAPTURE_BATTERY_LEVEL 3 00026 #define ADC_STATE_CAPTURE_LINE_SCAN 4 00027 #define ADC_STATE_CAPTURE_I_HBRIDGE_A 5 00028 #define ADC_STATE_CAPTURE_I_HBRIDGE_B 6 00029 00030 #define TFC_POT_0_ADC_CHANNEL 13 00031 #define TFC_POT_1_ADC_CHANNEL 12 00032 #define TFC_BAT_SENSE_CHANNEL 4 00033 #define TFC_LINESCAN0_ADC_CHANNEL 6 // Mux B 00034 #define TFC_LINESCAN1_ADC_CHANNEL 7 // Mux B 00035 // RAJOUTER UN CHANNEL ICI 00036 #define TFC_I_HBRIDGE_0_SENSE_ADC_CHANNEL 7 // Mux A 00037 #define TFC_I_HBRIDGE_1_SENSE_ADC_CHANNEL 3 // Mux A 00038 00039 00040 #define ADC0_irq_no 57 00041 #define ADC1_irq_no 58 00042 00043 #define ADC0_CHANA 19 // set to desired ADC0 channel trigger A 00044 #define ADC0_CHANB 20 // set to desired ADC0 channel trigger B 00045 00046 #define ADC1_CHANA 20 // set to desired ADC1 channel trigger A 20 defaults to potentiometer in TWRK60 00047 #define ADC1_CHANB 20 // set to desired ADC1 channel trigger B 00048 00049 #define ADC0_DLYA 0x2000 // ADC0 trigger A delay 00050 #define ADC0_DLYB 0x4000 // ADC0 trigger B delay 00051 #define ADC1_DLYA 0x6000 // ADC1 trigger A delay 00052 #define ADC1_DLYB 0x7fff // ADC1 trigger B delay 00053 00054 00055 #define ADC0A_DONE 0x01 00056 #define ADC0B_DONE 0x02 00057 #define ADC1A_DONE 0x04 00058 #define ADC1B_DONE 0x08 00059 00060 00061 // Bit shifting of bitfiled is already taken into account so 00062 // bitfiled values are always represented as relative to their position. 00063 00064 /************************* #Defines ******************************************/ 00065 00066 #define A 0x0 00067 #define B 0x1 00068 00069 /////// NOTE: the following defines relate to the ADC register definitions 00070 /////// and the content follows the reference manual, using the same symbols. 00071 00072 00073 //// ADCSC1 (register) 00074 00075 // Conversion Complete (COCO) mask 00076 #define COCO_COMPLETE ADC_SC1_COCO_MASK 00077 #define COCO_NOT 0x00 00078 00079 // ADC interrupts: enabled, or disabled. 00080 #define AIEN_ON ADC_SC1_AIEN_MASK 00081 #define AIEN_OFF 0x00 00082 00083 // Differential or Single ended ADC input 00084 #define DIFF_SINGLE 0x00 00085 #define DIFF_DIFFERENTIAL ADC_SC1_DIFF_MASK 00086 00087 //// ADCCFG1 00088 00089 // Power setting of ADC 00090 #define ADLPC_LOW ADC_CFG1_ADLPC_MASK 00091 #define ADLPC_NORMAL 0x00 00092 00093 // Clock divisor 00094 #define ADIV_1 0x00 00095 #define ADIV_2 0x01 00096 #define ADIV_4 0x02 00097 #define ADIV_8 0x03 00098 00099 // Long samle time, or Short sample time 00100 #define ADLSMP_LONG ADC_CFG1_ADLSMP_MASK 00101 #define ADLSMP_SHORT 0x00 00102 00103 // How many bits for the conversion? 8, 12, 10, or 16 (single ended). 00104 #define MODE_8 0x00 00105 #define MODE_12 0x01 00106 #define MODE_10 0x02 00107 #define MODE_16 0x03 00108 00109 00110 00111 // ADC Input Clock Source choice? Bus clock, Bus clock/2, "altclk", or the 00112 // ADC's own asynchronous clock for less noise 00113 #define ADICLK_BUS 0x00 00114 #define ADICLK_BUS_2 0x01 00115 #define ADICLK_ALTCLK 0x02 00116 #define ADICLK_ADACK 0x03 00117 00118 //// ADCCFG2 00119 00120 // Select between B or A channels 00121 #define MUXSEL_ADCB ADC_CFG2_MUXSEL_MASK 00122 #define MUXSEL_ADCA 0x00 00123 00124 // Ansync clock output enable: enable, or disable the output of it 00125 #define ADACKEN_ENABLED ADC_CFG2_ADACKEN_MASK 00126 #define ADACKEN_DISABLED 0x00 00127 00128 // High speed or low speed conversion mode 00129 #define ADHSC_HISPEED ADC_CFG2_ADHSC_MASK 00130 #define ADHSC_NORMAL 0x00 00131 00132 // Long Sample Time selector: 20, 12, 6, or 2 extra clocks for a longer sample time 00133 #define ADLSTS_20 0x00 00134 #define ADLSTS_12 0x01 00135 #define ADLSTS_6 0x02 00136 #define ADLSTS_2 0x03 00137 00138 ////ADCSC2 00139 00140 // Read-only status bit indicating conversion status 00141 #define ADACT_ACTIVE ADC_SC2_ADACT_MASK 00142 #define ADACT_INACTIVE 0x00 00143 00144 // Trigger for starting conversion: Hardware trigger, or software trigger. 00145 // For using PDB, the Hardware trigger option is selected. 00146 #define ADTRG_HW ADC_SC2_ADTRG_MASK 00147 #define ADTRG_SW 0x00 00148 00149 // ADC Compare Function Enable: Disabled, or Enabled. 00150 #define ACFE_DISABLED 0x00 00151 #define ACFE_ENABLED ADC_SC2_ACFE_MASK 00152 00153 // Compare Function Greater Than Enable: Greater, or Less. 00154 #define ACFGT_GREATER ADC_SC2_ACFGT_MASK 00155 #define ACFGT_LESS 0x00 00156 00157 // Compare Function Range Enable: Enabled or Disabled. 00158 #define ACREN_ENABLED ADC_SC2_ACREN_MASK 00159 #define ACREN_DISABLED 0x00 00160 00161 // DMA enable: enabled or disabled. 00162 #define DMAEN_ENABLED ADC_SC2_DMAEN_MASK 00163 #define DMAEN_DISABLED 0x00 00164 00165 // Voltage Reference selection for the ADC conversions 00166 // (***not*** the PGA which uses VREFO only). 00167 // VREFH and VREFL (0) , or VREFO (1). 00168 00169 #define REFSEL_EXT 0x00 00170 #define REFSEL_ALT 0x01 00171 #define REFSEL_RES 0x02 /* reserved */ 00172 #define REFSEL_RES_EXT 0x03 /* reserved but defaults to Vref */ 00173 00174 ////ADCSC3 00175 00176 // Calibration begin or off 00177 #define CAL_BEGIN ADC_SC3_CAL_MASK 00178 #define CAL_OFF 0x00 00179 00180 // Status indicating Calibration failed, or normal success 00181 #define CALF_FAIL ADC_SC3_CALF_MASK 00182 #define CALF_NORMAL 0x00 00183 00184 // ADC to continously convert, or do a sinle conversion 00185 #define ADCO_CONTINUOUS ADC_SC3_ADCO_MASK 00186 #define ADCO_SINGLE 0x00 00187 00188 // Averaging enabled in the ADC, or not. 00189 #define AVGE_ENABLED ADC_SC3_AVGE_MASK 00190 #define AVGE_DISABLED 0x00 00191 00192 // How many to average prior to "interrupting" the MCU? 4, 8, 16, or 32 00193 #define AVGS_4 0x00 00194 #define AVGS_8 0x01 00195 #define AVGS_16 0x02 00196 #define AVGS_32 0x03 00197 00198 ////PGA 00199 00200 // PGA enabled or not? 00201 #define PGAEN_ENABLED ADC_PGA_PGAEN_MASK 00202 #define PGAEN_DISABLED 0x00 00203 00204 // Chopper stabilization of the amplifier, or not. 00205 #define PGACHP_CHOP ADC_PGA_PGACHP_MASK 00206 #define PGACHP_NOCHOP 0x00 00207 00208 // PGA in low power mode, or normal mode. 00209 #define PGALP_LOW ADC_PGA_PGALP_MASK 00210 #define PGALP_NORMAL 0x00 00211 00212 // Gain of PGA. Selectable from 1 to 64. 00213 #define PGAG_1 0x00 00214 #define PGAG_2 0x01 00215 #define PGAG_4 0x02 00216 #define PGAG_8 0x03 00217 #define PGAG_16 0x04 00218 #define PGAG_32 0x05 00219 #define PGAG_64 0x06 00220 00221 /////////// The above values fit into the structure below to select ADC/PGA 00222 /////////// configuration desired: 00223 00224 typedef struct adc_cfg { 00225 uint8_t CONFIG1; 00226 uint8_t CONFIG2; 00227 uint16_t COMPARE1; 00228 uint16_t COMPARE2; 00229 uint8_t STATUS2; 00230 uint8_t STATUS3; 00231 uint8_t STATUS1A; 00232 uint8_t STATUS1B; 00233 uint32_t PGA; 00234 } *tADC_ConfigPtr, tADC_Config ; 00235 00236 00237 #define CAL_BLK_NUMREC 18 00238 00239 typedef struct adc_cal { 00240 00241 uint16_t OFS; 00242 uint16_t PG; 00243 uint16_t MG; 00244 uint8_t CLPD; 00245 uint8_t CLPS; 00246 uint16_t CLP4; 00247 uint16_t CLP3; 00248 uint8_t CLP2; 00249 uint8_t CLP1; 00250 uint8_t CLP0; 00251 uint8_t dummy; 00252 uint8_t CLMD; 00253 uint8_t CLMS; 00254 uint16_t CLM4; 00255 uint16_t CLM3; 00256 uint8_t CLM2; 00257 uint8_t CLM1; 00258 uint8_t CLM0; 00259 } tADC_Cal_Blk ; 00260 00261 typedef struct ADC_MemMap { 00262 uint32_t SC1[2]; /**< ADC Status and Control Registers 1, array offset: 0x0, array step: 0x4 */ 00263 uint32_t CFG1; /**< ADC Configuration Register 1, offset: 0x8 */ 00264 uint32_t CFG2; /**< ADC Configuration Register 2, offset: 0xC */ 00265 uint32_t R[2]; /**< ADC Data Result Register, array offset: 0x10, array step: 0x4 */ 00266 uint32_t CV1; /**< Compare Value Registers, offset: 0x18 */ 00267 uint32_t CV2; /**< Compare Value Registers, offset: 0x1C */ 00268 uint32_t SC2; /**< Status and Control Register 2, offset: 0x20 */ 00269 uint32_t SC3; /**< Status and Control Register 3, offset: 0x24 */ 00270 uint32_t OFS; /**< ADC Offset Correction Register, offset: 0x28 */ 00271 uint32_t PG; /**< ADC Plus-Side Gain Register, offset: 0x2C */ 00272 uint32_t MG; /**< ADC Minus-Side Gain Register, offset: 0x30 */ 00273 uint32_t CLPD; /**< ADC Plus-Side General Calibration Value Register, offset: 0x34 */ 00274 uint32_t CLPS; /**< ADC Plus-Side General Calibration Value Register, offset: 0x38 */ 00275 uint32_t CLP4; /**< ADC Plus-Side General Calibration Value Register, offset: 0x3C */ 00276 uint32_t CLP3; /**< ADC Plus-Side General Calibration Value Register, offset: 0x40 */ 00277 uint32_t CLP2; /**< ADC Plus-Side General Calibration Value Register, offset: 0x44 */ 00278 uint32_t CLP1; /**< ADC Plus-Side General Calibration Value Register, offset: 0x48 */ 00279 uint32_t CLP0; /**< ADC Plus-Side General Calibration Value Register, offset: 0x4C */ 00280 uint8_t RESERVED_0[4]; 00281 uint32_t CLMD; /**< ADC Minus-Side General Calibration Value Register, offset: 0x54 */ 00282 uint32_t CLMS; /**< ADC Minus-Side General Calibration Value Register, offset: 0x58 */ 00283 uint32_t CLM4; /**< ADC Minus-Side General Calibration Value Register, offset: 0x5C */ 00284 uint32_t CLM3; /**< ADC Minus-Side General Calibration Value Register, offset: 0x60 */ 00285 uint32_t CLM2; /**< ADC Minus-Side General Calibration Value Register, offset: 0x64 */ 00286 uint32_t CLM1; /**< ADC Minus-Side General Calibration Value Register, offset: 0x68 */ 00287 uint32_t CLM0; /**< ADC Minus-Side General Calibration Value Register, offset: 0x6C */ 00288 } volatile *ADC_MemMapPtr; 00289 00290 00291 00292 /* ADC - Register accessors */ 00293 #define ADC_SC1_REG(base,index) ((base)->SC1[index]) 00294 #define ADC_CFG1_REG(base) ((base)->CFG1) 00295 #define ADC_CFG2_REG(base) ((base)->CFG2) 00296 #define ADC_R_REG(base,index) ((base)->R[index]) 00297 #define ADC_CV1_REG(base) ((base)->CV1) 00298 #define ADC_CV2_REG(base) ((base)->CV2) 00299 #define ADC_SC2_REG(base) ((base)->SC2) 00300 #define ADC_SC3_REG(base) ((base)->SC3) 00301 #define ADC_OFS_REG(base) ((base)->OFS) 00302 #define ADC_PG_REG(base) ((base)->PG) 00303 #define ADC_MG_REG(base) ((base)->MG) 00304 #define ADC_CLPD_REG(base) ((base)->CLPD) 00305 #define ADC_CLPS_REG(base) ((base)->CLPS) 00306 #define ADC_CLP4_REG(base) ((base)->CLP4) 00307 #define ADC_CLP3_REG(base) ((base)->CLP3) 00308 #define ADC_CLP2_REG(base) ((base)->CLP2) 00309 #define ADC_CLP1_REG(base) ((base)->CLP1) 00310 #define ADC_CLP0_REG(base) ((base)->CLP0) 00311 #define ADC_CLMD_REG(base) ((base)->CLMD) 00312 #define ADC_CLMS_REG(base) ((base)->CLMS) 00313 #define ADC_CLM4_REG(base) ((base)->CLM4) 00314 #define ADC_CLM3_REG(base) ((base)->CLM3) 00315 #define ADC_CLM2_REG(base) ((base)->CLM2) 00316 #define ADC_CLM1_REG(base) ((base)->CLM1) 00317 #define ADC_CLM0_REG(base) ((base)->CLM0) 00318 00319 #define ADC0_BASE_PTR ((ADC_MemMapPtr)0x4003B000u) 00320 /** Array initializer of ADC peripheral base pointers */ 00321 #define ADC_BASE_PTRS { ADC0_BASE_PTR } 00322 00323 00324 float _ServoDutyCycleMin; 00325 float _ServoDutyCycleMax; 00326 float _ServoPeriod; 00327 00328 volatile uint16_t QueuedServo0Val; 00329 volatile uint16_t QueuedServo1Val; 00330 00331 volatile uint16_t *LineScanImage0WorkingBuffer; 00332 volatile uint16_t *LineScanImage1WorkingBuffer; 00333 00334 volatile uint16_t LineScanImage0Buffer[2][128]; 00335 volatile uint16_t LineScanImage1Buffer[2][128]; 00336 volatile uint8_t LineScanWorkingBuffer; 00337 00338 volatile uint16_t * TFC_LineScanImage0; 00339 volatile uint16_t * TFC_LineScanImage1; 00340 volatile uint8_t TFC_LineScanImageReady; 00341 00342 volatile uint16_t PotADC_Value[2]; 00343 volatile uint16_t BatSenseADC_Value; 00344 volatile uint16_t CurrentADC_State; 00345 volatile uint8_t CurrentLineScanPixel; 00346 volatile uint8_t CurrentLineScanChannel; 00347 volatile uint32_t TFC_ServoTicker; 00348 00349 volatile uint16_t HBridge_0_current_value; 00350 volatile uint16_t HBridge_1_current_value; 00351 00352 00353 void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle); 00354 void TFC_InitLineScanCamera(); 00355 uint8_t ADC_Cal(ADC_MemMapPtr adcmap); 00356 void ADC_Config_Alt(ADC_MemMapPtr adcmap, tADC_ConfigPtr ADC_CfgPtr); 00357 void ADC_Read_Cal(ADC_MemMapPtr adcmap, tADC_Cal_Blk *blk); 00358 void TFC_InitADC0(); 00359 void TFC_InitADC_System(); 00360 void TFC_GPIO_Init(); 00361 void ADC0_Handler(); 00362 void TPM1_Handler(); 00363 00364 00365 void TFC_Init() 00366 { 00367 00368 TFC_GPIO_Init(); 00369 00370 TFC_InitADC_System(); // Always call this before the Servo init function.... The IRQ for the Servo code modifies ADC registers and the clocks need enable to the ADC peripherals 1st! 00371 00372 TFC_InitLineScanCamera(); 00373 00374 TFC_InitServos(SERVO_MIN_PULSE_WIDTH_DEFAULT , SERVO_MAX_PULSE_WIDTH_DEFAULT, SERVO_DEFAULT_PERIOD); 00375 00376 TFC_ServoTicker = 0; 00377 00378 TFC_InitMotorPWM(FTM0_DEFAULT_SWITCHING_FREQUENCY); 00379 00380 } 00381 00382 00383 void TFC_GPIO_Init() 00384 { 00385 00386 //enable Clocks to all ports 00387 00388 SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK; 00389 00390 //Setup Pins as GPIO 00391 PORTE->PCR[21] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; 00392 PORTE->PCR[20] = PORT_PCR_MUX(1); 00393 00394 //Port for Pushbuttons 00395 PORTC->PCR[13] = PORT_PCR_MUX(1); 00396 PORTC->PCR[17] = PORT_PCR_MUX(1); 00397 00398 00399 //Ports for DIP Switches 00400 PORTE->PCR[2] = PORT_PCR_MUX(1); 00401 PORTE->PCR[3] = PORT_PCR_MUX(1); 00402 PORTE->PCR[4] = PORT_PCR_MUX(1); 00403 PORTE->PCR[5] = PORT_PCR_MUX(1); 00404 00405 //Ports for LEDs 00406 PORTB->PCR[8] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; 00407 PORTB->PCR[9] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; 00408 PORTB->PCR[10] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; 00409 PORTB->PCR[11] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; 00410 00411 00412 //Setup the output pins 00413 PTE->PDDR = TFC_HBRIDGE_EN_LOC; 00414 PTB->PDDR = TFC_BAT_LED0_LOC | TFC_BAT_LED1_LOC | TFC_BAT_LED2_LOC | TFC_BAT_LED3_LOC; 00415 00416 TFC_SetBatteryLED(0); 00417 TFC_HBRIDGE_DISABLE; 00418 } 00419 00420 void TFC_SetBatteryLED(uint8_t Value) 00421 { 00422 if(Value & 0x01) 00423 TFC_BAT_LED0_ON; 00424 else 00425 TFC_BAT_LED0_OFF; 00426 00427 if(Value & 0x02) 00428 TFC_BAT_LED1_ON; 00429 else 00430 TFC_BAT_LED1_OFF; 00431 00432 if(Value & 0x04) 00433 TFC_BAT_LED2_ON; 00434 else 00435 TFC_BAT_LED2_OFF; 00436 00437 if(Value & 0x08) 00438 TFC_BAT_LED3_ON; 00439 else 00440 TFC_BAT_LED3_OFF; 00441 } 00442 00443 uint8_t TFC_GetDIP_Switch() 00444 { 00445 uint8_t DIP_Val=0; 00446 00447 DIP_Val = (PTE->PDIR>>2) & 0xF; 00448 00449 return DIP_Val; 00450 } 00451 00452 uint8_t TFC_ReadPushButton(uint8_t Index) 00453 { 00454 if(Index == 0) { 00455 return TFC_PUSH_BUTTON_0_PRESSED; 00456 } else { 00457 return TFC_PUSH_BUTTON_1_PRESSED; 00458 } 00459 } 00460 00461 extern "C" void TPM1_IRQHandler() 00462 { 00463 //Clear the overflow mask if set. According to the reference manual, we clear by writing a logic one! 00464 if(TPM1->SC & TPM_SC_TOF_MASK) 00465 TPM1->SC |= TPM_SC_TOF_MASK; 00466 00467 //Dump the queued values to the timer channels 00468 TPM1->CONTROLS[0].CnV = QueuedServo0Val; 00469 TPM1->CONTROLS[1].CnV = QueuedServo1Val; 00470 00471 00472 //Prime the next ADC capture cycle 00473 TAOS_SI_HIGH; 00474 //Prime the ADC pump and start capturing POT 0 00475 CurrentADC_State = ADC_STATE_CAPTURE_POT_0; 00476 00477 ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux 00478 ADC0->SC1[0] = TFC_POT_0_ADC_CHANNEL | ADC_SC1_AIEN_MASK; //Start the State machine at POT0 00479 00480 //Flag that a new cervo cycle will start 00481 if (TFC_ServoTicker < 0xffffffff)//if servo tick less than max value, count up... 00482 TFC_ServoTicker++; 00483 00484 } 00485 00486 00487 void TFC_InitServos(float PulseWidthMin, float PulseWidthMax, float ServoPeriod) 00488 { 00489 00490 SIM->SCGC5 |= SIM_SCGC5_PORTB_MASK; 00491 00492 _ServoPeriod = ServoPeriod; 00493 _ServoDutyCycleMin = PulseWidthMin/ServoPeriod; 00494 _ServoDutyCycleMax = PulseWidthMax/ServoPeriod; 00495 00496 //Clock Setup for the TPM requires a couple steps. 00497 SIM->SCGC6 &= ~SIM_SCGC6_TPM1_MASK; 00498 //1st, set the clock mux 00499 //See Page 124 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 00500 SIM->SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;// We Want MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012) 00501 SIM->SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK); 00502 SIM->SOPT2 |= SIM_SOPT2_TPMSRC(1); 00503 00504 //Enable the Clock to the FTM0 Module 00505 //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 00506 SIM->SCGC6 |= SIM_SCGC6_TPM1_MASK; 00507 00508 //The TPM Module has Clock. Now set up the peripheral 00509 00510 //Blow away the control registers to ensure that the counter is not running 00511 TPM1->SC = 0; 00512 TPM1->CONF = 0; 00513 00514 //While the counter is disabled we can setup the prescaler 00515 00516 TPM1->SC = TPM_SC_PS(FTM1_CLK_PRESCALE); 00517 TPM1->SC |= TPM_SC_TOIE_MASK; //Enable Interrupts for the Timer Overflow 00518 00519 //Setup the mod register to get the correct PWM Period 00520 00521 TPM1->MOD = (SystemCoreClock/(1<<(FTM1_CLK_PRESCALE))) * _ServoPeriod; 00522 //Setup Channels 0 and 1 00523 00524 TPM1->CONTROLS[0].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; 00525 TPM1->CONTROLS[1].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; 00526 00527 00528 //Set the Default duty cycle to servo neutral 00529 TFC_SetServo(0, 0.0); 00530 TFC_SetServo(1, 0.0); 00531 00532 //Enable the TPM COunter 00533 TPM1->SC |= TPM_SC_CMOD(1); 00534 00535 //Enable TPM1 IRQ on the NVIC 00536 00537 //NVIC_SetVector(TPM1_IRQn,(uint32_t)TPM1_Handler); 00538 NVIC_EnableIRQ(TPM1_IRQn); 00539 00540 //Enable the FTM functions on the the port 00541 00542 PORTB->PCR[0] = PORT_PCR_MUX(3); 00543 PORTB->PCR[1] = PORT_PCR_MUX(3); 00544 00545 } 00546 00547 00548 void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle) 00549 { 00550 switch(ServoNumber) { 00551 default: 00552 case 0: 00553 00554 QueuedServo0Val = TPM1->MOD * DutyCycle; 00555 00556 break; 00557 00558 case 1: 00559 00560 QueuedServo1Val = TPM1->MOD * DutyCycle; 00561 00562 break; 00563 } 00564 } 00565 00566 void TFC_SetServo(uint8_t ServoNumber, float Position) 00567 { 00568 TFC_SetServoDutyCycle(ServoNumber , 00569 ((Position + 1.0)/2) * ((_ServoDutyCycleMax - _ServoDutyCycleMin))+_ServoDutyCycleMin) ; 00570 00571 } 00572 00573 //******************************************************************************************************** 00574 //******************************************************************************************************** 00575 //******************************************************************************************************** 00576 // _____ _____ ______ _ _ _ _ _____ _______ _____ ____ _ _ _____ 00577 // /\ | __ \ / ____| | ____| | | | \ | |/ ____|__ __|_ _/ __ \| \ | |/ ____| 00578 // / \ | | | | | | |__ | | | | \| | | | | | || | | | \| | (___ 00579 // / /\ \ | | | | | | __| | | | | . ` | | | | | || | | | . ` |\___ \ 00580 // / ____ \| |__| | |____ | | | |__| | |\ | |____ | | _| || |__| | |\ |____) | 00581 // /_/ \_\_____/ \_____| |_| \____/|_| \_|\_____| |_| |_____\____/|_| \_|_____/ 00582 // ******************************************************************************************************** 00583 // ******************************************************************************************************** 00584 // ******************************************************************************************************** 00585 00586 00587 00588 00589 00590 uint8_t ADC_Cal(ADC_MemMapPtr adcmap) 00591 { 00592 00593 uint16_t cal_var; 00594 00595 ADC_SC2_REG(adcmap) &= ~ADC_SC2_ADTRG_MASK ; // Enable Software Conversion Trigger for Calibration Process - ADC0_SC2 = ADC0_SC2 | ADC_SC2_ADTRGW(0); 00596 ADC_SC3_REG(adcmap) &= ( ~ADC_SC3_ADCO_MASK & ~ADC_SC3_AVGS_MASK ); // set single conversion, clear avgs bitfield for next writing 00597 ADC_SC3_REG(adcmap) |= ( ADC_SC3_AVGE_MASK | ADC_SC3_AVGS(AVGS_32) ); // Turn averaging ON and set at max value ( 32 ) 00598 00599 00600 ADC_SC3_REG(adcmap) |= ADC_SC3_CAL_MASK ; // Start CAL 00601 while ( (ADC_SC1_REG(adcmap,A) & ADC_SC1_COCO_MASK ) == COCO_NOT ); // Wait calibration end 00602 00603 if ((ADC_SC3_REG(adcmap)& ADC_SC3_CALF_MASK) == CALF_FAIL ) { 00604 return(1); // Check for Calibration fail error and return 00605 } 00606 // Calculate plus-side calibration 00607 cal_var = 0x00; 00608 00609 cal_var = ADC_CLP0_REG(adcmap); 00610 cal_var += ADC_CLP1_REG(adcmap); 00611 cal_var += ADC_CLP2_REG(adcmap); 00612 cal_var += ADC_CLP3_REG(adcmap); 00613 cal_var += ADC_CLP4_REG(adcmap); 00614 cal_var += ADC_CLPS_REG(adcmap); 00615 00616 cal_var = cal_var/2; 00617 cal_var |= 0x8000; // Set MSB 00618 00619 ADC_PG_REG(adcmap) = ADC_PG_PG(cal_var); 00620 00621 00622 // Calculate minus-side calibration 00623 cal_var = 0x00; 00624 00625 cal_var = ADC_CLM0_REG(adcmap); 00626 cal_var += ADC_CLM1_REG(adcmap); 00627 cal_var += ADC_CLM2_REG(adcmap); 00628 cal_var += ADC_CLM3_REG(adcmap); 00629 cal_var += ADC_CLM4_REG(adcmap); 00630 cal_var += ADC_CLMS_REG(adcmap); 00631 00632 cal_var = cal_var/2; 00633 00634 cal_var |= 0x8000; // Set MSB 00635 00636 ADC_MG_REG(adcmap) = ADC_MG_MG(cal_var); 00637 00638 ADC_SC3_REG(adcmap) &= ~ADC_SC3_CAL_MASK ; /* Clear CAL bit */ 00639 00640 return(0); 00641 } 00642 00643 00644 void ADC_Config_Alt(ADC_MemMapPtr adcmap, tADC_ConfigPtr ADC_CfgPtr) 00645 { 00646 ADC_CFG1_REG(adcmap) = ADC_CfgPtr->CONFIG1; 00647 ADC_CFG2_REG(adcmap) = ADC_CfgPtr->CONFIG2; 00648 ADC_CV1_REG(adcmap) = ADC_CfgPtr->COMPARE1; 00649 ADC_CV2_REG(adcmap) = ADC_CfgPtr->COMPARE2; 00650 ADC_SC2_REG(adcmap) = ADC_CfgPtr->STATUS2; 00651 ADC_SC3_REG(adcmap) = ADC_CfgPtr->STATUS3; 00652 //ADC_PGA_REG(adcmap) = ADC_CfgPtr->PGA; 00653 ADC_SC1_REG(adcmap,A)= ADC_CfgPtr->STATUS1A; 00654 ADC_SC1_REG(adcmap,B)= ADC_CfgPtr->STATUS1B; 00655 } 00656 00657 00658 void ADC_Read_Cal(ADC_MemMapPtr adcmap, tADC_Cal_Blk *blk) 00659 { 00660 blk->OFS = ADC_OFS_REG(adcmap); 00661 blk->PG = ADC_PG_REG(adcmap); 00662 blk->MG = ADC_MG_REG(adcmap); 00663 blk->CLPD = ADC_CLPD_REG(adcmap); 00664 blk->CLPS = ADC_CLPS_REG(adcmap); 00665 blk->CLP4 = ADC_CLP4_REG(adcmap); 00666 blk->CLP3 = ADC_CLP3_REG(adcmap); 00667 blk->CLP2 = ADC_CLP2_REG(adcmap); 00668 blk->CLP1 = ADC_CLP1_REG(adcmap); 00669 blk->CLP0 = ADC_CLP0_REG(adcmap); 00670 blk->CLMD = ADC_CLMD_REG(adcmap); 00671 blk->CLMS = ADC_CLMS_REG(adcmap); 00672 blk->CLM4 = ADC_CLM4_REG(adcmap); 00673 blk->CLM3 = ADC_CLM3_REG(adcmap); 00674 blk->CLM2 = ADC_CLM2_REG(adcmap); 00675 blk->CLM1 = ADC_CLM1_REG(adcmap); 00676 blk->CLM0 = ADC_CLM0_REG(adcmap); 00677 00678 } 00679 00680 00681 void TFC_InitADC0() 00682 { 00683 tADC_Config Master_Adc0_Config; 00684 00685 00686 SIM->SCGC6 |= (SIM_SCGC6_ADC0_MASK); 00687 00688 //Lets calibrate the ADC. 1st setup how the channel will be used. 00689 00690 00691 Master_Adc0_Config.CONFIG1 = ADLPC_NORMAL //No low power mode 00692 | ADC_CFG1_ADIV(ADIV_4) //divide input by 4 00693 | ADLSMP_LONG //long sample time 00694 | ADC_CFG1_MODE(MODE_12)//single ended 8-bit conversion 00695 | ADC_CFG1_ADICLK(ADICLK_BUS); 00696 00697 Master_Adc0_Config.CONFIG2 = MUXSEL_ADCA // select the A side of the ADC channel. 00698 | ADACKEN_DISABLED 00699 | ADHSC_HISPEED 00700 | ADC_CFG2_ADLSTS(ADLSTS_2);//Extra long sample Time (20 extra clocks) 00701 00702 00703 Master_Adc0_Config.COMPARE1 = 00000; // Comparators don't matter for calibration 00704 Master_Adc0_Config.COMPARE1 = 0xFFFF; 00705 00706 Master_Adc0_Config.STATUS2 = ADTRG_HW //hardware triggers for calibration 00707 | ACFE_DISABLED //disable comparator 00708 | ACFGT_GREATER 00709 | ACREN_ENABLED 00710 | DMAEN_DISABLED //Disable DMA 00711 | ADC_SC2_REFSEL(REFSEL_EXT); //External Reference 00712 00713 Master_Adc0_Config.STATUS3 = CAL_OFF 00714 | ADCO_SINGLE 00715 | AVGE_ENABLED; 00716 // | ADC_SC3_AVGS(AVGS_4); 00717 00718 Master_Adc0_Config.PGA = 0; // Disable the PGA 00719 00720 00721 // Configure ADC as it will be used, but because ADC_SC1_ADCH is 31, 00722 // the ADC will be inactive. Channel 31 is just disable function. 00723 // There really is no channel 31. 00724 00725 Master_Adc0_Config.STATUS1A = AIEN_ON | DIFF_SINGLE | ADC_SC1_ADCH(31); 00726 00727 00728 ADC_Config_Alt(ADC0_BASE_PTR, &Master_Adc0_Config); // config ADC 00729 00730 // Calibrate the ADC in the configuration in which it will be used: 00731 ADC_Cal(ADC0_BASE_PTR); // do the calibration 00732 00733 00734 Master_Adc0_Config.STATUS2 = ACFE_DISABLED //disable comparator 00735 | ACFGT_GREATER 00736 | ACREN_ENABLED 00737 | DMAEN_DISABLED //Disable DMA 00738 | ADC_SC2_REFSEL(REFSEL_EXT); //External Reference 00739 00740 Master_Adc0_Config.STATUS3 = CAL_OFF 00741 | ADCO_SINGLE; 00742 00743 00744 00745 ADC_Config_Alt(ADC0_BASE_PTR, &Master_Adc0_Config); 00746 } 00747 00748 00749 void TFC_InitADC_System() 00750 { 00751 00752 TFC_InitADC0(); 00753 00754 00755 //All Adc processing of the Pots and linescan will be done in the ADC0 IRQ! 00756 //A state machine will scan through the channels. 00757 //This is done to automate the linescan capture on Channel 0 to ensure that timing is very even 00758 CurrentADC_State = ADC_STATE_INIT; 00759 00760 //The pump will be primed with the TPM1 interrupt. upon timeout/interrupt it will set the SI signal high 00761 //for the camera and then start the conversions for the pots. 00762 00763 // NVIC_SetVector(ADC0_IRQn,(uint32_t)ADC0_Handler); 00764 NVIC_EnableIRQ(ADC0_IRQn); 00765 00766 } 00767 00768 extern "C" void ADC0_IRQHandler() 00769 { 00770 uint8_t Junk; 00771 00772 switch(CurrentADC_State) { 00773 default: 00774 Junk = ADC0->R[0]; 00775 break; 00776 00777 case ADC_STATE_CAPTURE_POT_0: 00778 00779 PotADC_Value[0] = ADC0->R[0]; 00780 ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux 00781 ADC0->SC1[0] = TFC_POT_1_ADC_CHANNEL | ADC_SC1_AIEN_MASK; 00782 CurrentADC_State = ADC_STATE_CAPTURE_POT_1; 00783 00784 break; 00785 00786 case ADC_STATE_CAPTURE_POT_1: 00787 00788 PotADC_Value[1] = ADC0->R[0]; 00789 00790 CurrentADC_State = ADC_STATE_CAPTURE_I_HBRIDGE_A; 00791 ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux 00792 ADC0->SC1[0] = TFC_I_HBRIDGE_0_SENSE_ADC_CHANNEL | ADC_SC1_AIEN_MASK; // HBRIDGE channel 00793 break; 00794 00795 00796 // NEW STATES 00797 case ADC_STATE_CAPTURE_I_HBRIDGE_A: 00798 // Read ADC value 00799 HBridge_0_current_value = ADC0->R[0]; 00800 // Change the ADC active channel 00801 ADC0->SC1[0] = TFC_I_HBRIDGE_1_SENSE_ADC_CHANNEL | ADC_SC1_AIEN_MASK; 00802 // Update the state machine 00803 CurrentADC_State = ADC_STATE_CAPTURE_I_HBRIDGE_B; 00804 break; 00805 00806 case ADC_STATE_CAPTURE_I_HBRIDGE_B: 00807 HBridge_1_current_value = ADC0->R[0]; 00808 ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux 00809 ADC0->SC1[0] = TFC_BAT_SENSE_CHANNEL| ADC_SC1_AIEN_MASK; 00810 CurrentADC_State = ADC_STATE_CAPTURE_BATTERY_LEVEL; 00811 break; 00812 00813 case ADC_STATE_CAPTURE_BATTERY_LEVEL: 00814 00815 BatSenseADC_Value = ADC0->R[0]; 00816 00817 //Now we will start the sequence for the Linescan camera 00818 00819 TAOS_CLK_HIGH; 00820 00821 for(Junk = 0; Junk<50; Junk++) { 00822 } 00823 00824 TAOS_SI_LOW; 00825 00826 00827 CurrentLineScanPixel = 0; 00828 CurrentLineScanChannel = 0; 00829 CurrentADC_State = ADC_STATE_CAPTURE_LINE_SCAN; 00830 ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux 00831 ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK; 00832 00833 break; 00834 00835 case ADC_STATE_CAPTURE_LINE_SCAN: 00836 00837 if(CurrentLineScanPixel<128) { 00838 if(CurrentLineScanChannel == 0) { 00839 LineScanImage0WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0]; 00840 ADC0->SC1[0] = TFC_LINESCAN1_ADC_CHANNEL | ADC_SC1_AIEN_MASK; 00841 CurrentLineScanChannel = 1; 00842 00843 } else { 00844 LineScanImage1WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0]; 00845 ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK; 00846 CurrentLineScanChannel = 0; 00847 CurrentLineScanPixel++; 00848 00849 TAOS_CLK_LOW; 00850 for(Junk = 0; Junk<50; Junk++) { 00851 } 00852 TAOS_CLK_HIGH; 00853 00854 } 00855 00856 } else { 00857 // done with the capture sequence. we can wait for the PIT0 IRQ to restart 00858 00859 TAOS_CLK_HIGH; 00860 00861 for(Junk = 0; Junk<50; Junk++) { 00862 } 00863 00864 TAOS_CLK_LOW; 00865 CurrentADC_State = ADC_STATE_INIT; 00866 00867 //swap the buffer 00868 00869 if(LineScanWorkingBuffer == 0) { 00870 LineScanWorkingBuffer = 1; 00871 00872 LineScanImage0WorkingBuffer = &LineScanImage0Buffer[1][0]; 00873 LineScanImage1WorkingBuffer = &LineScanImage1Buffer[1][0]; 00874 00875 TFC_LineScanImage0 = &LineScanImage0Buffer[0][0]; 00876 TFC_LineScanImage1 = &LineScanImage1Buffer[0][0]; 00877 } else { 00878 LineScanWorkingBuffer = 0; 00879 LineScanImage0WorkingBuffer = &LineScanImage0Buffer[0][0]; 00880 LineScanImage1WorkingBuffer = &LineScanImage1Buffer[0][0]; 00881 00882 TFC_LineScanImage0 = &LineScanImage0Buffer[1][0]; 00883 TFC_LineScanImage1 = &LineScanImage1Buffer[1][0]; 00884 } 00885 00886 TFC_LineScanImageReady++; 00887 } 00888 00889 break; 00890 } 00891 00892 } 00893 00894 void TFC_InitLineScanCamera() 00895 { 00896 SIM->SCGC5 |= SIM_SCGC5_PORTE_MASK | SIM_SCGC5_PORTD_MASK; //Make sure the clock is enabled for PORTE; 00897 PORTE->PCR[1] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; //Enable GPIO on on the pin for the CLOCK Signal 00898 PORTD->PCR[7] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK; //Enable GPIO on on the pin for SI signal 00899 00900 PORTD->PCR[5] = PORT_PCR_MUX(0); //Make sure AO signal goes to an analog input 00901 PORTD->PCR[6] = PORT_PCR_MUX(0); //Make sure AO signal goes to an analog input 00902 00903 //Make sure the Clock and SI pins are outputs 00904 PTD->PDDR |= (1<<7); 00905 PTE->PDDR |= (1<<1); 00906 00907 TAOS_CLK_LOW; 00908 TAOS_SI_LOW; 00909 00910 LineScanWorkingBuffer = 0; 00911 00912 LineScanImage0WorkingBuffer = &LineScanImage0Buffer[LineScanWorkingBuffer][0]; 00913 LineScanImage1WorkingBuffer = &LineScanImage1Buffer[LineScanWorkingBuffer][0]; 00914 00915 TFC_LineScanImage0 = &LineScanImage0Buffer[1][0]; 00916 TFC_LineScanImage1 = &LineScanImage1Buffer[1][0]; 00917 } 00918 00919 00920 00921 00922 00923 /** Initialized TPM0 to be used for generating PWM signals for the the dual drive motors. This method is called in the TFC constructor with a default value of 4000.0Hz 00924 * 00925 * @param SwitchingFrequency PWM Switching Frequency in floating point format. Pick something between 1000 and 9000. Maybe you can modulate it and make a tune. 00926 */ 00927 void TFC_InitMotorPWM(float SwitchingFrequency) 00928 { 00929 //Clock Setup for the TPM requires a couple steps. 00930 00931 //1st, set the clock mux 00932 //See Page 124 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 00933 SIM->SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;// We Want MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012) 00934 SIM->SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK); 00935 SIM->SOPT2 |= SIM_SOPT2_TPMSRC(1); //We want the MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012) 00936 00937 00938 //Enable the Clock to the FTM0 Module 00939 //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 00940 SIM->SCGC6 |= SIM_SCGC6_TPM0_MASK; 00941 00942 //The TPM Module has Clock. Now set up the peripheral 00943 00944 //Blow away the control registers to ensure that the counter is not running 00945 TPM0->SC = 0; 00946 TPM0->CONF = 0; 00947 00948 //While the counter is disabled we can setup the prescaler 00949 00950 TPM0->SC = TPM_SC_PS(FTM0_CLK_PRESCALE); 00951 00952 //Setup the mod register to get the correct PWM Period 00953 00954 TPM0->MOD = (uint32_t)((float)(FTM0_CLOCK/(1<<FTM0_CLK_PRESCALE))/SwitchingFrequency); 00955 00956 //Setup Channels 0,1,2,3 00957 TPM0->CONTROLS[0].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; 00958 TPM0->CONTROLS[1].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSA_MASK; // invert the second PWM signal for a complimentary output; 00959 TPM0->CONTROLS[2].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; 00960 TPM0->CONTROLS[3].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSA_MASK; // invert the second PWM signal for a complimentary output; 00961 00962 //Enable the Counter 00963 00964 //Set the Default duty cycle to 50% duty cycle 00965 TFC_SetMotorPWM(0.0,0.0); 00966 00967 //Enable the TPM COunter 00968 TPM0->SC |= TPM_SC_CMOD(1); 00969 00970 //Enable the FTM functions on the the port 00971 PORTC->PCR[1] = PORT_PCR_MUX(4); 00972 PORTC->PCR[2] = PORT_PCR_MUX(4); 00973 PORTC->PCR[3] = PORT_PCR_MUX(4); 00974 PORTC->PCR[4] = PORT_PCR_MUX(4); 00975 00976 } 00977 00978 void TFC_SetMotorPWM(float MotorA , float MotorB) 00979 { 00980 if(MotorA>1.0) 00981 MotorA = 1.0; 00982 else if(MotorA<-1.0) 00983 MotorA = -1.0; 00984 00985 if(MotorB>1.0) 00986 MotorB = 1.0; 00987 else if(MotorB<-1.0) 00988 MotorB = -1.0; 00989 00990 TPM0->CONTROLS[2].CnV = (uint16_t) ((float)TPM0->MOD * (float)((MotorA + 1.0)/2.0)); 00991 TPM0->CONTROLS[3].CnV = TPM0->CONTROLS[2].CnV; 00992 TPM0->CONTROLS[0].CnV = (uint16_t) ((float)TPM0->MOD * (float)((MotorB + 1.0)/2.0)); 00993 TPM0->CONTROLS[1].CnV = TPM0->CONTROLS[0].CnV; 00994 00995 } 00996 00997 //Pot Reading is Scaled to return a value of -1.0 to 1.0 00998 float TFC_ReadPot(uint8_t Channel) 00999 { 01000 if(Channel == 0) 01001 return ((float)PotADC_Value[0]/-((float)ADC_MAX_CODE/2.0))+1.0; 01002 else 01003 return ((float)PotADC_Value[1]/-((float)ADC_MAX_CODE/2.0))+1.0; 01004 } 01005 01006 float TFC_ReadBatteryVoltage() 01007 { 01008 return (((float)BatSenseADC_Value/(float)(ADC_MAX_CODE)) * 3.0);// * ((47000.0+10000.0)/10000.0); 01009 } 01010 01011 float TFC_ReadMotorCurrent(uint8_t motorChannel) 01012 { 01013 if(motorChannel) 01014 return ((float)(HBridge_0_current_value)/(float)(ADC_MAX_CODE));//TODO : Coefficient ? 01015 else 01016 return ((float)(HBridge_1_current_value)/(float)(ADC_MAX_CODE));//TODO : Coefficient ? 01017 } 01018 01019 01020 void TFC_SetBatteryLED_Level(uint8_t BattLevel) 01021 { 01022 switch(BattLevel) 01023 { 01024 default: 01025 case 0: 01026 TFC_BAT_LED0_OFF; 01027 TFC_BAT_LED1_OFF; 01028 TFC_BAT_LED2_OFF; 01029 TFC_BAT_LED3_OFF; 01030 break; 01031 01032 case 1: 01033 TFC_BAT_LED0_ON; 01034 TFC_BAT_LED1_OFF; 01035 TFC_BAT_LED2_OFF; 01036 TFC_BAT_LED3_OFF; 01037 break; 01038 01039 case 2: 01040 TFC_BAT_LED0_ON; 01041 TFC_BAT_LED1_ON; 01042 TFC_BAT_LED2_OFF; 01043 TFC_BAT_LED3_OFF; 01044 break; 01045 01046 case 3: 01047 TFC_BAT_LED0_ON; 01048 TFC_BAT_LED1_ON; 01049 TFC_BAT_LED2_ON; 01050 TFC_BAT_LED3_OFF; 01051 break; 01052 01053 case 4: 01054 TFC_BAT_LED0_ON; 01055 TFC_BAT_LED1_ON; 01056 TFC_BAT_LED2_ON; 01057 TFC_BAT_LED3_ON; 01058 break; 01059 01060 } 01061 }
Generated on Thu Jul 14 2022 01:08:00 by
1.7.2