Revision 9:0c6d78c56091, committed 2017-06-01

Comitter:

Clarkk

Date:

Thu Jun 01 13:16:37 2017 +0000

Parent:

8:24430a0d7fd8

Commit message:

Few changes

Changed in this revision

--- 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)
 {

--- a/TFC.h	Thu Feb 11 14:58:08 2016 +0000
+++ b/TFC.h	Thu Jun 01 13:16:37 2017 +0000
@@ -85,6 +85,12 @@
 #define TAOS_SI_HIGH   PTD->PSOR = (1<<7)
 #define TAOS_SI_LOW    PTD->PCOR = (1<<7)
 
+#define ADC_MASK_CAPTURE_POT_0                  1
+#define ADC_MASK_CAPTURE_POT_1                  2
+#define ADC_MASK_CAPTURE_BATTERY_LEVEL          4
+#define ADC_MASK_CAPTURE_MOTOR_CURRENT          8
+#define ADC_MASK_CAPTURE_LINE_SCAN              16
+#define ADC_MASK_CAPTURE_ALL                    31
 
 /**
 
@@ -211,13 +217,29 @@
 */
 float TFC_ReadPot(uint8_t Channel);
 
+
+uint16_t TFC_ReadBatteryVoltageRaw();
+uint16_t TFC_ReadMotACurrentRaw();
+uint16_t TFC_ReadMotBCurrentRaw();
+
+
 /** Gets the current battery voltage
 *
 *  @returns    Battery voltage in floating point form.
 */
 float TFC_ReadBatteryVoltage();
 
+/** Gets motor A current
+*
+*  @returns    Current value in amps in floating point form.
+*/
+float TFC_ReadMotACurrent();
 
+/** Gets motor B current
+*
+*  @returns    Current value in amps in floating point form.
+*/
+float TFC_ReadMotBCurrent();
 
 /** Sets the Battery level indiciate
 *
@@ -226,6 +248,16 @@
 */
 void TFC_SetBatteryLED_Level(uint8_t BattLevel);
 
+/** Stop camera exposure
+*
+*/
+void TFC_StopCameraExposure();
+
+/** Start ADC convertion of all analog inputs
+*
+*  @param mask  The mask of convertions to perform
+*/
+void TFC_StartDataAcquisition(uint8_t mask);
 
 /** Pointer to two channels of line scan camera data.   Each channel is 128 points of uint8_t's.  Note that the underlying implementation is ping-pong buffer  These pointers will point to the 
 *inactive buffer.   
@@ -241,10 +273,12 @@
 */
 
 extern volatile uint8_t TFC_LineScanImageReady;
-
+extern volatile uint8_t TFC_Pot0Ready;
+extern volatile uint8_t TFC_Pot1Ready;
+extern volatile uint8_t TFC_BatteryVoltageReady;
+extern volatile uint8_t TFC_MotorCurrentReady;
 
-
-
+extern FunctionPointer TFC_ConvertionReadyCallback;
 
 /** @} */