Pascal Sandrez
Custom version for NXP cup car
Fork of
FRDM-TFC
by 
Eli Hughes
Diff: TFC.cpp
- Revision:
- 9:0c6d78c56091
- Parent:
- 8:24430a0d7fd8
--- a/TFC.cpp Thu Feb 11 14:58:08 2016 +0000
+++ b/TFC.cpp Thu Jun 01 13:16:37 2017 +0000
@@ -4,8 +4,8 @@
#define FTM1_CLK_PRESCALE 6// Prescale Selector value - see comments in Status Control (SC) section for more details
#define SERVO_DEFAULT_PERIOD (float)(.020) // Desired Frequency of PWM Signal - Here 50Hz => 20ms period
// use these to dial in servo steering to your particular servo
-#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
-#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
+#define SERVO_MIN_PULSE_WIDTH_DEFAULT (float)(.0005) // The number here should be be *pulse width* in seconds to move servo to its left limit
+#define SERVO_MAX_PULSE_WIDTH_DEFAULT (float)(.00150) // The number here should be be *pulse width* in seconds to move servo to its left limit
#define FTM0_CLOCK (SystemCoreClock/2)
@@ -24,13 +24,16 @@
#define ADC_STATE_CAPTURE_POT_1 2
#define ADC_STATE_CAPTURE_BATTERY_LEVEL 3
#define ADC_STATE_CAPTURE_LINE_SCAN 4
-
+#define ADC_STATE_CAPTURE_MOTOR_A_CURRENT 5
+#define ADC_STATE_CAPTURE_MOTOR_B_CURRENT 6
#define TFC_POT_0_ADC_CHANNEL 13
#define TFC_POT_1_ADC_CHANNEL 12
#define TFC_BAT_SENSE_CHANNEL 4
#define TFC_LINESCAN0_ADC_CHANNEL 6
#define TFC_LINESCAN1_ADC_CHANNEL 7
+#define TFC_IFB_MOTA_ADC_CHANNEL 7
+#define TFC_IFB_MOTB_ADC_CHANNEL 3
#define ADC0_irq_no 57
@@ -214,14 +217,6 @@
#define PGAG_32 0x05
#define PGAG_64 0x06
-
-#define ADC_STATE_INIT 0
-#define ADC_STATE_CAPTURE_POT_0 1
-#define ADC_STATE_CAPTURE_POT_1 2
-#define ADC_STATE_CAPTURE_BATTERY_LEVEL 3
-#define ADC_STATE_CAPTURE_LINE_SCAN 4
-
-
/////////// The above values fit into the structure below to select ADC/PGA
/////////// configuration desired:
@@ -241,7 +236,6 @@
#define CAL_BLK_NUMREC 18
typedef struct adc_cal {
-
uint16_t OFS;
uint16_t PG;
uint16_t MG;
@@ -343,13 +337,22 @@
volatile uint16_t * TFC_LineScanImage1;
volatile uint8_t TFC_LineScanImageReady;
+volatile uint8_t ConvertionTasks;
volatile uint16_t PotADC_Value[2];
volatile uint16_t BatSenseADC_Value;
volatile uint16_t CurrentADC_State;
+volatile uint16_t MotACurrentADC_Value;
+volatile uint16_t MotBCurrentADC_Value;
volatile uint8_t CurrentLineScanPixel;
volatile uint8_t CurrentLineScanChannel;
volatile uint32_t TFC_ServoTicker;
+volatile uint8_t TFC_Pot0Ready;
+volatile uint8_t TFC_Pot1Ready;
+volatile uint8_t TFC_BatteryVoltageReady;
+volatile uint8_t TFC_MotorCurrentReady;
+
+FunctionPointer TFC_ConvertionReadyCallback;
void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle);
void TFC_InitLineScanCamera();
@@ -383,9 +386,7 @@
void TFC_GPIO_Init()
{
-
//enable Clocks to all ports
-
SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK;
//Setup Pins as GPIO
@@ -396,7 +397,6 @@
PORTC->PCR[13] = PORT_PCR_MUX(1);
PORTC->PCR[17] = PORT_PCR_MUX(1);
-
//Ports for DIP Switches
PORTE->PCR[2] = PORT_PCR_MUX(1);
PORTE->PCR[3] = PORT_PCR_MUX(1);
@@ -409,7 +409,6 @@
PORTB->PCR[10] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;
PORTB->PCR[11] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;
-
//Setup the output pins
PTE->PDDR = TFC_HBRIDGE_EN_LOC;
PTB->PDDR = TFC_BAT_LED0_LOC | TFC_BAT_LED1_LOC | TFC_BAT_LED2_LOC | TFC_BAT_LED3_LOC;
@@ -468,26 +467,17 @@
//Dump the queued values to the timer channels
TPM1->CONTROLS[0].CnV = QueuedServo0Val;
TPM1->CONTROLS[1].CnV = QueuedServo1Val;
-
+
+ //TFC_StartDataAcquisition();
- //Prime the next ADC capture cycle
- TAOS_SI_HIGH;
- //Prime the ADC pump and start capturing POT 0
- CurrentADC_State = ADC_STATE_CAPTURE_POT_0;
-
- ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
- ADC0->SC1[0] = TFC_POT_0_ADC_CHANNEL | ADC_SC1_AIEN_MASK; //Start the State machine at POT0
-
- //Flag that a new cervo cycle will start
+ //Flag that a new servo cycle will start
if (TFC_ServoTicker < 0xffffffff)//if servo tick less than max value, count up...
TFC_ServoTicker++;
}
-
void TFC_InitServos(float PulseWidthMin, float PulseWidthMax, float ServoPeriod)
{
-
SIM->SCGC5 |= SIM_SCGC5_PORTB_MASK;
_ServoPeriod = ServoPeriod;
@@ -766,6 +756,109 @@
}
+void TFC_StopCameraExposure()
+{
+ TAOS_SI_HIGH;
+}
+
+void TFC_StartADC_Pot0()
+{
+ // start capturing POT 0
+ CurrentADC_State = ADC_STATE_CAPTURE_POT_0;
+
+ ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
+ ADC0->SC1[0] = TFC_POT_0_ADC_CHANNEL | ADC_SC1_AIEN_MASK; //Start the State machine at POT0
+}
+
+void TFC_StartADC_Pot1()
+{
+ CurrentADC_State = ADC_STATE_CAPTURE_POT_1;
+ ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
+ ADC0->SC1[0] = TFC_POT_1_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+}
+
+void TFC_StartADC_Battery()
+{
+ CurrentADC_State = ADC_STATE_CAPTURE_BATTERY_LEVEL;
+ ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux
+ ADC0->SC1[0] = TFC_BAT_SENSE_CHANNEL| ADC_SC1_AIEN_MASK;
+}
+
+void TFC_StartADC_MotACurrent()
+{
+ CurrentADC_State = ADC_STATE_CAPTURE_MOTOR_A_CURRENT;
+ ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
+ ADC0->SC1[0] = TFC_IFB_MOTA_ADC_CHANNEL| ADC_SC1_AIEN_MASK;
+}
+
+void TFC_StartADC_MotBCurrent()
+{
+ CurrentADC_State = ADC_STATE_CAPTURE_MOTOR_B_CURRENT;
+ //ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux
+ ADC0->SC1[0] = TFC_IFB_MOTB_ADC_CHANNEL| ADC_SC1_AIEN_MASK;
+}
+
+void TFC_StartADC_LineScan()
+{
+ uint8_t i;
+ CurrentADC_State = ADC_STATE_CAPTURE_LINE_SCAN;
+
+ //Prime the next ADC capture cycle
+ TAOS_SI_HIGH;
+ for(i = 0; i<50; i++) { // 50 counts is ~120ns
+ }
+ TAOS_CLK_HIGH;
+ for(i = 0; i<50; i++) {
+ }
+ TAOS_SI_LOW;
+
+ CurrentLineScanPixel = 0;
+ CurrentLineScanChannel = 0;
+ ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux
+ ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+}
+
+void TFC_StartNextConvertion()
+{
+ if(ConvertionTasks&ADC_MASK_CAPTURE_POT_0)
+ {
+ TFC_StartADC_Pot0();
+ ConvertionTasks ^= ADC_MASK_CAPTURE_POT_0;
+ return;
+ }
+ if(ConvertionTasks & ADC_MASK_CAPTURE_POT_1)
+ {
+ TFC_StartADC_Pot1();
+ ConvertionTasks ^= ADC_MASK_CAPTURE_POT_1;
+ return;
+ }
+ if(ConvertionTasks & ADC_MASK_CAPTURE_BATTERY_LEVEL)
+ {
+ TFC_StartADC_Battery();
+ ConvertionTasks ^= ADC_MASK_CAPTURE_BATTERY_LEVEL;
+ return;
+ }
+ if(ConvertionTasks & ADC_MASK_CAPTURE_MOTOR_CURRENT)
+ {
+ TFC_StartADC_MotACurrent();
+ ConvertionTasks ^= ADC_MASK_CAPTURE_MOTOR_CURRENT;
+ return;
+ }
+ if(ConvertionTasks & ADC_MASK_CAPTURE_LINE_SCAN)
+ {
+ TFC_StartADC_LineScan();
+ ConvertionTasks ^= ADC_MASK_CAPTURE_LINE_SCAN;
+ return;
+ }
+ CurrentADC_State = ADC_STATE_INIT;
+}
+
+void TFC_StartDataAcquisition(uint8_t mask)
+{
+ ConvertionTasks = mask;
+ TFC_StartNextConvertion();
+}
+
extern "C" void ADC0_IRQHandler()
{
uint8_t Junk;
@@ -776,54 +869,56 @@
break;
case ADC_STATE_CAPTURE_POT_0:
-
PotADC_Value[0] = ADC0->R[0];
- ADC0->CFG2 &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
- ADC0->SC1[0] = TFC_POT_1_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
- CurrentADC_State = ADC_STATE_CAPTURE_POT_1;
-
+ TFC_StartNextConvertion();
+ TFC_Pot0Ready++;
break;
case ADC_STATE_CAPTURE_POT_1:
-
PotADC_Value[1] = ADC0->R[0];
- ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux
- ADC0->SC1[0] = TFC_BAT_SENSE_CHANNEL| ADC_SC1_AIEN_MASK;
- CurrentADC_State = ADC_STATE_CAPTURE_BATTERY_LEVEL;
-
+ TFC_StartNextConvertion();
+ TFC_Pot1Ready++;
break;
case ADC_STATE_CAPTURE_BATTERY_LEVEL:
-
BatSenseADC_Value = ADC0->R[0];
-
- //Now we will start the sequence for the Linescan camera
-
- TAOS_CLK_HIGH;
-
- for(Junk = 0; Junk<50; Junk++) {
- }
+ TFC_StartNextConvertion();
+ TFC_BatteryVoltageReady++;
+ TFC_ConvertionReadyCallback.call();
+ break;
- TAOS_SI_LOW;
-
+ case ADC_STATE_CAPTURE_MOTOR_A_CURRENT:
+ MotACurrentADC_Value = ADC0->R[0];
+ TFC_StartADC_MotBCurrent();
+ break;
- CurrentLineScanPixel = 0;
- CurrentLineScanChannel = 0;
- CurrentADC_State = ADC_STATE_CAPTURE_LINE_SCAN;
- ADC0->CFG2 |= ADC_CFG2_MUXSEL_MASK; //Select the B side of the mux
- ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
-
+ case ADC_STATE_CAPTURE_MOTOR_B_CURRENT:
+ MotBCurrentADC_Value = ADC0->R[0];
+ TFC_StartNextConvertion();
+ TFC_MotorCurrentReady++;
+ TFC_ConvertionReadyCallback.call();
break;
case ADC_STATE_CAPTURE_LINE_SCAN:
-
- if(CurrentLineScanPixel<128) {
- if(CurrentLineScanChannel == 0) {
+ if(CurrentLineScanPixel<128)
+ {
+ LineScanImage0WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0];
+ ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+ CurrentLineScanPixel++;
+
+ TAOS_CLK_LOW;
+ for(Junk = 0; Junk<50; Junk++) {
+ }
+ TAOS_CLK_HIGH;
+
+ /* if(CurrentLineScanChannel == 0) // Pixel of camera 0 has been converted
+ {
LineScanImage0WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0];
ADC0->SC1[0] = TFC_LINESCAN1_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
CurrentLineScanChannel = 1;
-
- } else {
+ }
+ else // Pixel of camera 1 has been converted
+ {
LineScanImage1WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0];
ADC0->SC1[0] = TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
CurrentLineScanChannel = 0;
@@ -833,45 +928,40 @@
for(Junk = 0; Junk<50; Junk++) {
}
TAOS_CLK_HIGH;
-
- }
-
- } else {
+ }*/
+ }
+ else
+ {
// done with the capture sequence. we can wait for the PIT0 IRQ to restart
-
TAOS_CLK_HIGH;
-
for(Junk = 0; Junk<50; Junk++) {
}
-
TAOS_CLK_LOW;
- CurrentADC_State = ADC_STATE_INIT;
+ TFC_StartNextConvertion();
//swap the buffer
-
if(LineScanWorkingBuffer == 0) {
LineScanWorkingBuffer = 1;
LineScanImage0WorkingBuffer = &LineScanImage0Buffer[1][0];
- LineScanImage1WorkingBuffer = &LineScanImage1Buffer[1][0];
+ //LineScanImage1WorkingBuffer = &LineScanImage1Buffer[1][0];
TFC_LineScanImage0 = &LineScanImage0Buffer[0][0];
- TFC_LineScanImage1 = &LineScanImage1Buffer[0][0];
+ //TFC_LineScanImage1 = &LineScanImage1Buffer[0][0];
} else {
LineScanWorkingBuffer = 0;
LineScanImage0WorkingBuffer = &LineScanImage0Buffer[0][0];
- LineScanImage1WorkingBuffer = &LineScanImage1Buffer[0][0];
+ //LineScanImage1WorkingBuffer = &LineScanImage1Buffer[0][0];
TFC_LineScanImage0 = &LineScanImage0Buffer[1][0];
- TFC_LineScanImage1 = &LineScanImage1Buffer[1][0];
+ //TFC_LineScanImage1 = &LineScanImage1Buffer[1][0];
}
TFC_LineScanImageReady++;
+ TFC_ConvertionReadyCallback.call();
}
-
break;
}
-
}
void TFC_InitLineScanCamera()
@@ -899,10 +989,6 @@
TFC_LineScanImage1 = &LineScanImage1Buffer[1][0];
}
-
-
-
-
/** 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
*
* @param SwitchingFrequency PWM Switching Frequency in floating point format. Pick something between 1000 and 9000. Maybe you can modulate it and make a tune.
@@ -970,11 +1056,31 @@
else if(MotorB<-1.0)
MotorB = -1.0;
- TPM0->CONTROLS[2].CnV = (uint16_t) ((float)TPM0->MOD * (float)((MotorA + 1.0)/2.0));
+/* TPM0->CONTROLS[2].CnV = (uint16_t) ((float)TPM0->MOD * (float)((MotorA + 1.0)/2.0));
TPM0->CONTROLS[3].CnV = TPM0->CONTROLS[2].CnV;
TPM0->CONTROLS[0].CnV = (uint16_t) ((float)TPM0->MOD * (float)((MotorB + 1.0)/2.0));
- TPM0->CONTROLS[1].CnV = TPM0->CONTROLS[0].CnV;
+ TPM0->CONTROLS[1].CnV = TPM0->CONTROLS[0].CnV;*/
+ if(MotorA >= 0)
+ {
+ TPM0->CONTROLS[2].CnV = 0xffff; // recirculation by high side to have current measurement
+ TPM0->CONTROLS[3].CnV = (uint16_t) ((float)TPM0->MOD * (float)MotorA); // channel is inverted
+ }
+ else
+ {
+ TPM0->CONTROLS[2].CnV = (uint16_t) ((float)TPM0->MOD * (float)(1.0 + MotorA));
+ TPM0->CONTROLS[3].CnV = 0; // recirculation by high side to have current measurement (channel is inverted)
+ }
+ if(MotorB >= 0)
+ {
+ TPM0->CONTROLS[0].CnV = 0xffff; // recirculation by high side to have current measurement
+ TPM0->CONTROLS[1].CnV = (uint16_t) ((float)TPM0->MOD * (float)MotorB); // channel is inverted
+ }
+ else
+ {
+ TPM0->CONTROLS[0].CnV = (uint16_t) ((float)TPM0->MOD * (float)(1.0 + MotorB));
+ TPM0->CONTROLS[1].CnV = 0; // recirculation by high side to have current measurement (channel is inverted)
+ }
}
//Pot Reading is Scaled to return a value of -1.0 to 1.0
@@ -986,11 +1092,35 @@
return ((float)PotADC_Value[1]/-((float)ADC_MAX_CODE/2.0))+1.0;
}
+uint16_t TFC_ReadBatteryVoltageRaw()
+{
+ return BatSenseADC_Value;
+}
+
+uint16_t TFC_ReadMotACurrentRaw()
+{
+ return MotACurrentADC_Value;
+}
+
+uint16_t TFC_ReadMotBCurrentRaw()
+{
+ return MotBCurrentADC_Value;
+}
+
float TFC_ReadBatteryVoltage()
{
- return (((float)BatSenseADC_Value/(float)(ADC_MAX_CODE)) * 3.0);// * ((47000.0+10000.0)/10000.0);
+ return (((float)BatSenseADC_Value/(float)(ADC_MAX_CODE)) * 18.81);// * ((47000.0+10000.0)/10000.0*3.3)
}
+float TFC_ReadMotACurrent()
+{
+ return (((float)MotACurrentADC_Value/(float)(ADC_MAX_CODE)) * 5.625);// * 3.3 / 220 * 375
+}
+
+float TFC_ReadMotBCurrent()
+{
+ return (((float)MotBCurrentADC_Value/(float)(ADC_MAX_CODE)) * 5.625);// * 3.3 / 220 * 375
+}
void TFC_SetBatteryLED_Level(uint8_t BattLevel)
{