Revision 19:4869b10a962e, committed 2014-07-02

Comitter:

backman

Date:

Wed Jul 02 03:23:07 2014 +0000

Parent:

18:eb675df59c7f

Commit message:

wang

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BX-adc.cpp	Wed Jul 02 03:23:07 2014 +0000
@@ -0,0 +1,97 @@
+#ifdef TARGET_KLXX
+
+#include "BX-adc.h"
+#include "clk_freqs.h"
+
+#define MAX_FADC            6000000
+#define CHANNELS_A_SHIFT    5
+
+FastAnalogIn::FastAnalogIn(PinName pin, bool enabled)
+{
+    ADCnumber = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
+    if (ADCnumber == (ADCName)NC) {
+        error("ADC pin mapping failed");
+    }
+
+    SIM->SCGC6 |= SIM_SCGC6_ADC0_MASK;
+
+    uint32_t port = (uint32_t)pin >> PORT_SHIFT;
+    SIM->SCGC5 |= 1 << (SIM_SCGC5_PORTA_SHIFT + port);
+
+    uint32_t cfg2_muxsel = ADC_CFG2_MUXSEL_MASK;
+    if (ADCnumber & (1 << CHANNELS_A_SHIFT)) {
+        cfg2_muxsel = 0;
+    }
+    
+    // bus clk
+    uint32_t PCLK = bus_frequency();
+    uint32_t clkdiv;
+    for (clkdiv = 0; clkdiv < 4; clkdiv++) {
+        if ((PCLK >> clkdiv) <= MAX_FADC)
+            break;
+    }
+    if (clkdiv == 4)                    //Set max div
+        clkdiv = 0x7;
+
+    ADC0->SC1[1] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
+
+    ADC0->CFG1 = ADC_CFG1_ADIV(clkdiv & 0x3)    // Clock Divide Select: (Input Clock)/8
+               | ADC_CFG1_MODE(3)               // (16)bits Resolution
+               | ADC_CFG1_ADICLK(clkdiv >> 2);  // Input Clock: (Bus Clock)/2
+
+    ADC0->CFG2 = cfg2_muxsel            // ADxxb or ADxxa channels
+               | ADC_CFG2_ADACKEN_MASK  // Asynchronous Clock Output Enable
+               | ADC_CFG2_ADHSC_MASK;   // High-Speed Configuration
+
+    ADC0->SC2 = ADC_SC2_REFSEL(0);      // Default Voltage Reference
+
+    pinmap_pinout(pin, PinMap_ADC);
+
+    //Enable channel
+    running = false;
+    enable(enabled);
+}
+
+void FastAnalogIn::enable(bool enabled)
+{
+    //If currently not running
+    if (!running) {
+        if (enabled) {
+            //Enable the ADC channel
+            ADC0->SC3 |= ADC_SC3_ADCO_MASK;       // Enable continuous conversion
+            ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));  //Start conversion
+            running = true;
+        } else
+            disable();
+    }
+}
+ 
+void FastAnalogIn::disable( void )
+{
+    //If currently running
+    if (running) {
+        ADC0->SC3 &= ~ADC_SC3_ADCO_MASK;      // Disable continuous conversion
+    }
+    running = false;
+}
+
+uint16_t FastAnalogIn::read_u16()
+{
+    if (!running)
+    {
+        // start conversion
+        ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
+        // Wait Conversion Complete
+        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
+    }
+    if(running && ((ADC0->SC1[0]&ADC_SC1_ADCH_MASK) != (ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT)))))
+    {
+        running = false;
+        enable();
+        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
+    }
+    // Return value
+    return (uint16_t)ADC0->R[0];
+}
+
+#endif //defined TARGET_KLXX

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BX-adc.h	Wed Jul 02 03:23:07 2014 +0000
@@ -0,0 +1,117 @@
+#include "mbed.h"
+#include "pinmap.h"
+ 
+ 
+ 
+ /** A class similar to AnalogIn, only faster, for LPC1768, LPC408X and KLxx
+ *
+ * AnalogIn does a single conversion when you read a value (actually several conversions and it takes the median of that).
+ * This library runns the ADC conversion automatically in the background.
+ * When read is called, it immediatly returns the last sampled value.
+ *
+ * LPC1768 / LPC4088
+ * Using more ADC pins in continuous mode will decrease the conversion rate (LPC1768:200kHz/LPC4088:400kHz).
+ * If you need to sample one pin very fast and sometimes also need to do AD conversions on another pin,
+ * you can disable the continuous conversion on that ADC channel and still read its value.
+ *
+ * KLXX
+ * Multiple Fast instances can be declared of which only ONE can be continuous (all others must be non-continuous).
+ *
+ * When continuous conversion is disabled, a read will block until the conversion is complete
+ * (much like the regular AnalogIn library does).
+ * Each ADC channel can be enabled/disabled separately.
+ *
+ * IMPORTANT : It does not play nicely with regular AnalogIn objects, so either use this library or AnalogIn, not both at the same time!!
+ *
+ * Example for the KLxx processors:
+ * @code
+ * // Print messages when the AnalogIn is greater than 50%
+ *
+ * #include "mbed.h"
+ *
+ * FastAnalogIn temperature(PTC2); //Fast continuous sampling on PTC2
+ * FastAnalogIn speed(PTB3, 0);    //Fast non-continuous sampling on PTB3
+ *
+ * int main() {
+ *     while(1) {
+ *         if(temperature > 0.5) {
+ *             printf("Too hot! (%f) at speed %f", temperature.read(), speed.read());
+ *         }
+ *     }
+ * }
+ * @endcode
+ * Example for the LPC1768 processor:
+ * @code
+ * // Print messages when the AnalogIn is greater than 50%
+ *
+ * #include "mbed.h"
+ *
+ * FastAnalogIn temperature(p20);
+ *
+ * int main() {
+ *     while(1) {
+ *         if(temperature > 0.5) {
+ *             printf("Too hot! (%f)", temperature.read());
+ *         }
+ *     }
+ * }
+ * @endcode
+*/
+class FastAnalogIn {
+ 
+public:
+     /** Create a FastAnalogIn, connected to the specified pin
+     *
+     * @param pin AnalogIn pin to connect to
+     * @param enabled Enable the ADC channel (default = true)
+     */
+    FastAnalogIn( PinName pin, bool enabled = true );
+    
+    ~FastAnalogIn( void )
+    {
+        disable();
+    }
+    
+    /** Enable the ADC channel
+    *
+    * @param enabled Bool that is true for enable, false is equivalent to calling disable
+    */
+    void enable(bool enabled = true);
+    
+    /** Disable the ADC channel
+    *
+    * Disabling unused channels speeds up conversion in used channels. 
+    * When disabled you can still call read, that will do a single conversion (actually two since the first one always returns 0 for unknown reason).
+    * Then the function blocks until the value is read. This is handy when you sometimes needs a single conversion besides the automatic conversion
+    */
+    void disable( void );
+    
+    /** Returns the raw value
+    *
+    * @param return Unsigned integer with converted value
+    */
+    unsigned short read_u16( void );
+    
+    /** Returns the scaled value
+    *
+    * @param return Float with scaled converted value to 0.0-1.0
+    */
+    float read( void )
+    {
+        unsigned short value = read_u16();
+        return (float)value * (1.0f/65535.0f);
+    }
+    
+    /** An operator shorthand for read()
+    */
+    operator float() {
+        return read();
+    }
+ 
+    
+private:
+    bool running;    
+    char ADCnumber;
+    uint32_t *datareg;
+};
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TFC.cpp	Wed Jul 02 03:23:07 2014 +0000
@@ -0,0 +1,1038 @@
+#include "mbed.h"
+#include "TFC.h"
+
+#define FTM1_CLK_PRESCALE                                                                  6   // Prescale Selector value - see comments in Status Control (SC) section for more details
+#define SERVO_DEFAULT_PERIOD                                                   (float)(.010)   // Desired Frequency of PWM Signal - Here 50Hz => 20ms period
+#define TAOS_CLK_COUNT                                                                   200   // Number of cycles for CLK Signal on camera
+
+// use these to dial in servo steering to your particular servo
+#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)(.002)   // The number here should be be *pulse width* in seconds to move servo to its left limit 
+
+
+#define FTM0_CLOCK                                             (SystemCoreClock/2)
+#define FTM0_CLK_PRESCALE                                      (0)  // Prescale Selector value - see comments in Status Control (SC) section for more details
+#define FTM0_DEFAULT_SWITCHING_FREQUENCY                      (4000.0)
+
+#define ADC_MAX_CODE    (4095)
+
+#define TAOS_CLK_HIGH  PTE->PSOR = (1<<1)
+#define TAOS_CLK_LOW   PTE->PCOR = (1<<1)
+#define TAOS_SI_HIGH   PTD->PSOR = (1<<7)
+#define TAOS_SI_LOW    PTD->PCOR = (1<<7)
+
+#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
+
+
+#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 ADC0_irq_no   57
+#define ADC1_irq_no   58
+
+#define ADC0_CHANA    19                                    // set to desired ADC0 channel trigger A    
+#define ADC0_CHANB    20                                    // set to desired ADC0 channel trigger B    
+
+#define ADC1_CHANA    20                                    // set to desired ADC1 channel trigger A  20 defaults to potentiometer in TWRK60     
+#define ADC1_CHANB    20                                    // set to desired ADC1 channel trigger B
+
+#define ADC0_DLYA     0x2000                                // ADC0 trigger A delay 
+#define ADC0_DLYB     0x4000                                // ADC0 trigger B delay 
+#define ADC1_DLYA     0x6000                                // ADC1 trigger A delay
+#define ADC1_DLYB     0x7fff                                // ADC1 trigger B delay 
+
+
+#define ADC0A_DONE   0x01
+#define ADC0B_DONE   0x02
+#define ADC1A_DONE   0x04
+#define ADC1B_DONE   0x08
+
+
+// Bit shifting of bitfiled is already taken into account so
+// bitfiled values are always represented as relative to their position.
+
+/************************* #Defines ******************************************/
+
+#define A                 0x0
+#define B                 0x1
+
+/////// NOTE: the following defines relate to the ADC register definitions
+/////// and the content follows the reference manual, using the same symbols.
+
+
+//// ADCSC1 (register)
+
+// Conversion Complete (COCO) mask
+#define COCO_COMPLETE     ADC_SC1_COCO_MASK
+#define COCO_NOT          0x00
+
+// ADC interrupts: enabled, or disabled.
+#define AIEN_ON           ADC_SC1_AIEN_MASK
+#define AIEN_OFF          0x00
+
+// Differential or Single ended ADC input
+#define DIFF_SINGLE       0x00
+#define DIFF_DIFFERENTIAL ADC_SC1_DIFF_MASK
+
+//// ADCCFG1
+
+// Power setting of ADC
+#define ADLPC_LOW         ADC_CFG1_ADLPC_MASK
+#define ADLPC_NORMAL      0x00
+
+// Clock divisor
+#define ADIV_1            0x00
+#define ADIV_2            0x01
+#define ADIV_4            0x02
+#define ADIV_8            0x03
+
+// Long samle time, or Short sample time
+#define ADLSMP_LONG       ADC_CFG1_ADLSMP_MASK
+#define ADLSMP_SHORT      0x00
+
+// How many bits for the conversion?  8, 12, 10, or 16 (single ended).
+#define MODE_8            0x00
+#define MODE_12           0x01
+#define MODE_10           0x02
+#define MODE_16           0x03
+
+
+
+// ADC Input Clock Source choice? Bus clock, Bus clock/2, "altclk", or the
+//                                ADC's own asynchronous clock for less noise
+#define ADICLK_BUS        0x00
+#define ADICLK_BUS_2      0x01
+#define ADICLK_ALTCLK     0x02
+#define ADICLK_ADACK      0x03
+
+//// ADCCFG2
+
+// Select between B or A channels
+#define MUXSEL_ADCB       ADC_CFG2_MUXSEL_MASK
+#define MUXSEL_ADCA       0x00
+
+// Ansync clock output enable: enable, or disable the output of it
+#define ADACKEN_ENABLED   ADC_CFG2_ADACKEN_MASK
+#define ADACKEN_DISABLED  0x00
+
+// High speed or low speed conversion mode
+#define ADHSC_HISPEED     ADC_CFG2_ADHSC_MASK
+#define ADHSC_NORMAL      0x00
+
+// Long Sample Time selector: 20, 12, 6, or 2 extra clocks for a longer sample time
+#define ADLSTS_20          0x00
+#define ADLSTS_12          0x01
+#define ADLSTS_6           0x02
+#define ADLSTS_2           0x03
+
+////ADCSC2
+
+// Read-only status bit indicating conversion status
+#define ADACT_ACTIVE       ADC_SC2_ADACT_MASK
+#define ADACT_INACTIVE     0x00
+
+// Trigger for starting conversion: Hardware trigger, or software trigger.
+// For using PDB, the Hardware trigger option is selected.
+#define ADTRG_HW           ADC_SC2_ADTRG_MASK
+#define ADTRG_SW           0x00
+
+// ADC Compare Function Enable: Disabled, or Enabled.
+#define ACFE_DISABLED      0x00
+#define ACFE_ENABLED       ADC_SC2_ACFE_MASK
+
+// Compare Function Greater Than Enable: Greater, or Less.
+#define ACFGT_GREATER      ADC_SC2_ACFGT_MASK
+#define ACFGT_LESS         0x00
+
+// Compare Function Range Enable: Enabled or Disabled.
+#define ACREN_ENABLED      ADC_SC2_ACREN_MASK
+#define ACREN_DISABLED     0x00
+
+// DMA enable: enabled or disabled.
+#define DMAEN_ENABLED      ADC_SC2_DMAEN_MASK
+#define DMAEN_DISABLED     0x00
+
+// Voltage Reference selection for the ADC conversions
+// (***not*** the PGA which uses VREFO only).
+// VREFH and VREFL (0) , or VREFO (1).
+
+#define REFSEL_EXT         0x00
+#define REFSEL_ALT         0x01
+#define REFSEL_RES         0x02     /* reserved */
+#define REFSEL_RES_EXT     0x03     /* reserved but defaults to Vref */
+
+////ADCSC3
+
+// Calibration begin or off
+#define CAL_BEGIN          ADC_SC3_CAL_MASK
+#define CAL_OFF            0x00
+
+// Status indicating Calibration failed, or normal success
+#define CALF_FAIL          ADC_SC3_CALF_MASK
+#define CALF_NORMAL        0x00
+
+// ADC to continously convert, or do a sinle conversion
+#define ADCO_CONTINUOUS    ADC_SC3_ADCO_MASK
+#define ADCO_SINGLE        0x00
+
+// Averaging enabled in the ADC, or not.
+#define AVGE_ENABLED       ADC_SC3_AVGE_MASK
+#define AVGE_DISABLED      0x00
+
+// How many to average prior to "interrupting" the MCU?  4, 8, 16, or 32
+#define AVGS_4             0x00
+#define AVGS_8             0x01
+#define AVGS_16            0x02
+#define AVGS_32            0x03
+
+////PGA
+
+// PGA enabled or not?
+#define PGAEN_ENABLED      ADC_PGA_PGAEN_MASK
+#define PGAEN_DISABLED     0x00
+
+// Chopper stabilization of the amplifier, or not.
+#define PGACHP_CHOP        ADC_PGA_PGACHP_MASK
+#define PGACHP_NOCHOP      0x00
+
+// PGA in low power mode, or normal mode.
+#define PGALP_LOW          ADC_PGA_PGALP_MASK
+#define PGALP_NORMAL       0x00
+
+// Gain of PGA.  Selectable from 1 to 64.
+#define PGAG_1             0x00
+#define PGAG_2             0x01
+#define PGAG_4             0x02
+#define PGAG_8             0x03
+#define PGAG_16            0x04
+#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:
+
+typedef struct adc_cfg {
+    uint8_t  CONFIG1;
+    uint8_t  CONFIG2;
+    uint16_t COMPARE1;
+    uint16_t COMPARE2;
+    uint8_t  STATUS2;
+    uint8_t  STATUS3;
+    uint8_t  STATUS1A;
+    uint8_t  STATUS1B;
+    uint32_t PGA;
+} *tADC_ConfigPtr, tADC_Config ;
+
+
+#define CAL_BLK_NUMREC 18
+
+typedef struct adc_cal {
+
+    uint16_t  OFS;
+    uint16_t  PG;
+    uint16_t  MG;
+    uint8_t   CLPD;
+    uint8_t   CLPS;
+    uint16_t  CLP4;
+    uint16_t  CLP3;
+    uint8_t   CLP2;
+    uint8_t   CLP1;
+    uint8_t   CLP0;
+    uint8_t   dummy;
+    uint8_t   CLMD;
+    uint8_t   CLMS;
+    uint16_t  CLM4;
+    uint16_t  CLM3;
+    uint8_t   CLM2;
+    uint8_t   CLM1;
+    uint8_t   CLM0;
+} tADC_Cal_Blk ;
+
+typedef struct ADC_MemMap {
+    uint32_t SC1[2];                                 /**< ADC Status and Control Registers 1, array offset: 0x0, array step: 0x4 */
+    uint32_t CFG1;                                   /**< ADC Configuration Register 1, offset: 0x8 */
+    uint32_t CFG2;                                   /**< ADC Configuration Register 2, offset: 0xC */
+    uint32_t R[2];                                   /**< ADC Data Result Register, array offset: 0x10, array step: 0x4 */
+    uint32_t CV1;                                    /**< Compare Value Registers, offset: 0x18 */
+    uint32_t CV2;                                    /**< Compare Value Registers, offset: 0x1C */
+    uint32_t SC2;                                    /**< Status and Control Register 2, offset: 0x20 */
+    uint32_t SC3;                                    /**< Status and Control Register 3, offset: 0x24 */
+    uint32_t OFS;                                    /**< ADC Offset Correction Register, offset: 0x28 */
+    uint32_t PG;                                     /**< ADC Plus-Side Gain Register, offset: 0x2C */
+    uint32_t MG;                                     /**< ADC Minus-Side Gain Register, offset: 0x30 */
+    uint32_t CLPD;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x34 */
+    uint32_t CLPS;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x38 */
+    uint32_t CLP4;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x3C */
+    uint32_t CLP3;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x40 */
+    uint32_t CLP2;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x44 */
+    uint32_t CLP1;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x48 */
+    uint32_t CLP0;                                   /**< ADC Plus-Side General Calibration Value Register, offset: 0x4C */
+    uint8_t RESERVED_0[4];
+    uint32_t CLMD;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x54 */
+    uint32_t CLMS;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x58 */
+    uint32_t CLM4;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x5C */
+    uint32_t CLM3;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x60 */
+    uint32_t CLM2;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x64 */
+    uint32_t CLM1;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x68 */
+    uint32_t CLM0;                                   /**< ADC Minus-Side General Calibration Value Register, offset: 0x6C */
+} volatile *ADC_MemMapPtr;
+
+
+
+/* ADC - Register accessors */
+#define ADC_SC1_REG(base,index)                  ((base)->SC1[index])
+#define ADC_CFG1_REG(base)                       ((base)->CFG1)
+#define ADC_CFG2_REG(base)                       ((base)->CFG2)
+#define ADC_R_REG(base,index)                    ((base)->R[index])
+#define ADC_CV1_REG(base)                        ((base)->CV1)
+#define ADC_CV2_REG(base)                        ((base)->CV2)
+#define ADC_SC2_REG(base)                        ((base)->SC2)
+#define ADC_SC3_REG(base)                        ((base)->SC3)
+#define ADC_OFS_REG(base)                        ((base)->OFS)
+#define ADC_PG_REG(base)                         ((base)->PG)
+#define ADC_MG_REG(base)                         ((base)->MG)
+#define ADC_CLPD_REG(base)                       ((base)->CLPD)
+#define ADC_CLPS_REG(base)                       ((base)->CLPS)
+#define ADC_CLP4_REG(base)                       ((base)->CLP4)
+#define ADC_CLP3_REG(base)                       ((base)->CLP3)
+#define ADC_CLP2_REG(base)                       ((base)->CLP2)
+#define ADC_CLP1_REG(base)                       ((base)->CLP1)
+#define ADC_CLP0_REG(base)                       ((base)->CLP0)
+#define ADC_CLMD_REG(base)                       ((base)->CLMD)
+#define ADC_CLMS_REG(base)                       ((base)->CLMS)
+#define ADC_CLM4_REG(base)                       ((base)->CLM4)
+#define ADC_CLM3_REG(base)                       ((base)->CLM3)
+#define ADC_CLM2_REG(base)                       ((base)->CLM2)
+#define ADC_CLM1_REG(base)                       ((base)->CLM1)
+#define ADC_CLM0_REG(base)                       ((base)->CLM0)
+
+#define ADC0_BASE_PTR                            ((ADC_MemMapPtr)0x4003B000u)
+/** Array initializer of ADC peripheral base pointers */
+#define ADC_BASE_PTRS                            { ADC0_BASE_PTR }
+
+
+float _ServoDutyCycleMin;
+float _ServoDutyCycleMax;
+float _ServoPeriod;
+
+volatile uint16_t QueuedServo0Val;
+volatile uint16_t QueuedServo1Val;
+
+volatile uint16_t  *LineScanImage0WorkingBuffer;
+volatile uint16_t  *LineScanImage1WorkingBuffer;
+
+volatile uint16_t  LineScanImage0Buffer[2][128];
+volatile uint16_t  LineScanImage1Buffer[2][128];
+volatile uint8_t  LineScanWorkingBuffer;
+
+volatile uint16_t * TFC_LineScanImage0;
+volatile uint16_t * TFC_LineScanImage1;
+volatile uint8_t  TFC_LineScanImageReady;
+
+volatile uint16_t  PotADC_Value[2];
+volatile uint16_t  BatSenseADC_Value;
+volatile uint16_t  CurrentADC_State;
+volatile uint8_t  CurrentLineScanPixel;
+volatile uint8_t  CurrentLineScanChannel;
+volatile uint32_t TFC_ServoTicker;
+
+
+void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle);
+void TFC_InitLineScanCamera();
+uint8_t ADC_Cal(ADC_MemMapPtr adcmap);
+void ADC_Config_Alt(ADC_MemMapPtr adcmap, tADC_ConfigPtr ADC_CfgPtr);
+void ADC_Read_Cal(ADC_MemMapPtr adcmap, tADC_Cal_Blk *blk);
+void TFC_InitADC0();
+void TFC_InitADC_System();
+void TFC_GPIO_Init();
+void ADC0_Handler();
+void TPM1_Handler();
+
+
+void TFC_Init()
+{
+
+    TFC_GPIO_Init();
+
+    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!
+
+    TFC_InitLineScanCamera();
+
+  //  TFC_InitServos(SERVO_MIN_PULSE_WIDTH_DEFAULT , SERVO_MAX_PULSE_WIDTH_DEFAULT, SERVO_DEFAULT_PERIOD);
+    
+  //  TFC_ServoTicker = 0;
+
+  //  TFC_InitMotorPWM(FTM0_DEFAULT_SWITCHING_FREQUENCY);
+
+}
+
+
+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
+    PORTE->PCR[21] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;
+    PORTE->PCR[20] = PORT_PCR_MUX(1);
+
+    //Port for Pushbuttons
+    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);
+    PORTE->PCR[4] = PORT_PCR_MUX(1);
+    PORTE->PCR[5] = PORT_PCR_MUX(1);
+
+    //Ports for LEDs
+    PORTB->PCR[8] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;
+    PORTB->PCR[9] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;
+    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;
+
+    TFC_SetBatteryLED(0);
+    TFC_HBRIDGE_DISABLE;
+}
+
+void TFC_SetBatteryLED(uint8_t Value)
+{
+    if(Value & 0x01)
+        TFC_BAT_LED0_ON;
+    else
+        TFC_BAT_LED0_OFF;
+
+    if(Value & 0x02)
+        TFC_BAT_LED1_ON;
+    else
+        TFC_BAT_LED1_OFF;
+
+    if(Value & 0x04)
+        TFC_BAT_LED2_ON;
+    else
+        TFC_BAT_LED2_OFF;
+
+    if(Value & 0x08)
+        TFC_BAT_LED3_ON;
+    else
+        TFC_BAT_LED3_OFF;
+}
+
+uint8_t TFC_GetDIP_Switch()
+{
+    uint8_t DIP_Val=0;
+
+    DIP_Val = (PTE->PDIR>>2) & 0xF;
+
+    return DIP_Val;
+}
+
+uint8_t TFC_ReadPushButton(uint8_t Index)
+{
+    if(Index == 0) {
+        return TFC_PUSH_BUTTON_0_PRESSED;
+    } else {
+        return TFC_PUSH_BUTTON_1_PRESSED;
+    }
+}
+
+extern "C" void TPM1_IRQHandler()
+{
+    //Clear the overflow mask if set.   According to the reference manual, we clear by writing a logic one!
+    if(TPM1->SC & TPM_SC_TOF_MASK)
+        TPM1->SC |= TPM_SC_TOF_MASK;
+
+    //Dump the queued values to the timer channels
+    TPM1->CONTROLS[0].CnV = QueuedServo0Val;
+    TPM1->CONTROLS[1].CnV = QueuedServo1Val;
+
+
+    //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
+    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;
+    _ServoDutyCycleMin = PulseWidthMin/ServoPeriod;
+    _ServoDutyCycleMax = PulseWidthMax/ServoPeriod;
+
+    //Clock Setup for the TPM requires a couple steps.
+    SIM->SCGC6 &= ~SIM_SCGC6_TPM1_MASK;
+    //1st,  set the clock mux
+    //See Page 124 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012
+    SIM->SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;// We Want MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012)
+    SIM->SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK);
+    SIM->SOPT2 |= SIM_SOPT2_TPMSRC(1);
+
+    //Enable the Clock to the FTM0 Module
+    //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012
+    SIM->SCGC6 |= SIM_SCGC6_TPM1_MASK;
+
+    //The TPM Module has Clock.  Now set up the peripheral
+
+    //Blow away the control registers to ensure that the counter is not running
+    TPM1->SC = 0;
+    TPM1->CONF = 0;
+
+    //While the counter is disabled we can setup the prescaler
+
+    TPM1->SC = TPM_SC_PS(FTM1_CLK_PRESCALE);
+    TPM1->SC |= TPM_SC_TOIE_MASK; //Enable Interrupts for the Timer Overflow
+
+    //Setup the mod register to get the correct PWM Period
+
+    TPM1->MOD = (SystemCoreClock/(1<<(FTM1_CLK_PRESCALE))) * _ServoPeriod;
+    //Setup Channels 0 and 1
+
+    TPM1->CONTROLS[0].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
+    TPM1->CONTROLS[1].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
+
+
+    //Set the Default duty cycle to servo neutral
+    TFC_SetServo(0, 0.0);
+    TFC_SetServo(1, 0.0);
+
+    //Enable the TPM COunter
+    TPM1->SC |= TPM_SC_CMOD(1);
+
+    //Enable TPM1 IRQ on the NVIC
+
+    //NVIC_SetVector(TPM1_IRQn,(uint32_t)TPM1_Handler);
+    NVIC_EnableIRQ(TPM1_IRQn);
+
+    //Enable the FTM functions on the the port
+
+    PORTB->PCR[0] = PORT_PCR_MUX(3);
+    PORTB->PCR[1] = PORT_PCR_MUX(3);
+
+}
+
+
+void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle)
+{
+    switch(ServoNumber) {
+        default:
+        case 0:
+
+            QueuedServo0Val = TPM1->MOD * DutyCycle;
+
+            break;
+
+        case 1:
+
+            QueuedServo1Val = TPM1->MOD * DutyCycle;
+
+            break;
+    }
+}
+
+void TFC_SetServo(uint8_t ServoNumber, float Position)
+{
+    TFC_SetServoDutyCycle(ServoNumber ,
+                          ((Position + 1.0)/2)    *   ((_ServoDutyCycleMax - _ServoDutyCycleMin))+_ServoDutyCycleMin) ;
+
+}
+
+//********************************************************************************************************
+//********************************************************************************************************
+//********************************************************************************************************
+//           _____   _____       ______ _    _ _   _  _____ _______ _____ ____  _   _  _____
+//     /\   |  __ \ / ____|     |  ____| |  | | \ | |/ ____|__   __|_   _/ __ \| \ | |/ ____|
+//    /  \  | |  | | |          | |__  | |  | |  \| | |       | |    | || |  | |  \| | (___
+//   / /\ \ | |  | | |          |  __| | |  | | . ` | |       | |    | || |  | | . ` |\___ \
+//  / ____ \| |__| | |____      | |    | |__| | |\  | |____   | |   _| || |__| | |\  |____) |
+// /_/    \_\_____/ \_____|     |_|     \____/|_| \_|\_____|  |_|  |_____\____/|_| \_|_____/
+// ********************************************************************************************************
+// ********************************************************************************************************
+// ********************************************************************************************************
+
+
+
+
+
+uint8_t ADC_Cal(ADC_MemMapPtr adcmap)
+{
+
+    uint16_t cal_var;
+
+    ADC_SC2_REG(adcmap) &=  ~ADC_SC2_ADTRG_MASK ; // Enable Software Conversion Trigger for Calibration Process    - ADC0_SC2 = ADC0_SC2 | ADC_SC2_ADTRGW(0);
+    ADC_SC3_REG(adcmap) &= ( ~ADC_SC3_ADCO_MASK & ~ADC_SC3_AVGS_MASK ); // set single conversion, clear avgs bitfield for next writing
+    ADC_SC3_REG(adcmap) |= ( ADC_SC3_AVGE_MASK | ADC_SC3_AVGS(AVGS_32) );  // Turn averaging ON and set at max value ( 32 )
+
+
+    ADC_SC3_REG(adcmap) |= ADC_SC3_CAL_MASK ;      // Start CAL
+    while ( (ADC_SC1_REG(adcmap,A) & ADC_SC1_COCO_MASK ) == COCO_NOT ); // Wait calibration end
+
+    if ((ADC_SC3_REG(adcmap)& ADC_SC3_CALF_MASK) == CALF_FAIL ) {
+        return(1);    // Check for Calibration fail error and return
+    }
+    // Calculate plus-side calibration
+    cal_var = 0x00;
+
+    cal_var =  ADC_CLP0_REG(adcmap);
+    cal_var += ADC_CLP1_REG(adcmap);
+    cal_var += ADC_CLP2_REG(adcmap);
+    cal_var += ADC_CLP3_REG(adcmap);
+    cal_var += ADC_CLP4_REG(adcmap);
+    cal_var += ADC_CLPS_REG(adcmap);
+
+    cal_var = cal_var/2;
+    cal_var |= 0x8000; // Set MSB
+
+    ADC_PG_REG(adcmap) = ADC_PG_PG(cal_var);
+
+
+    // Calculate minus-side calibration
+    cal_var = 0x00;
+
+    cal_var =  ADC_CLM0_REG(adcmap);
+    cal_var += ADC_CLM1_REG(adcmap);
+    cal_var += ADC_CLM2_REG(adcmap);
+    cal_var += ADC_CLM3_REG(adcmap);
+    cal_var += ADC_CLM4_REG(adcmap);
+    cal_var += ADC_CLMS_REG(adcmap);
+
+    cal_var = cal_var/2;
+
+    cal_var |= 0x8000; // Set MSB
+
+    ADC_MG_REG(adcmap) = ADC_MG_MG(cal_var);
+
+    ADC_SC3_REG(adcmap) &= ~ADC_SC3_CAL_MASK ; /* Clear CAL bit */
+
+    return(0);
+}
+
+
+void ADC_Config_Alt(ADC_MemMapPtr adcmap, tADC_ConfigPtr ADC_CfgPtr)
+{
+    ADC_CFG1_REG(adcmap) = ADC_CfgPtr->CONFIG1;
+    ADC_CFG2_REG(adcmap) = ADC_CfgPtr->CONFIG2;
+    ADC_CV1_REG(adcmap)  = ADC_CfgPtr->COMPARE1;
+    ADC_CV2_REG(adcmap)  = ADC_CfgPtr->COMPARE2;
+    ADC_SC2_REG(adcmap)  = ADC_CfgPtr->STATUS2;
+    ADC_SC3_REG(adcmap)  = ADC_CfgPtr->STATUS3;
+//ADC_PGA_REG(adcmap)  = ADC_CfgPtr->PGA;
+    ADC_SC1_REG(adcmap,A)= ADC_CfgPtr->STATUS1A;
+    ADC_SC1_REG(adcmap,B)= ADC_CfgPtr->STATUS1B;
+}
+
+
+void ADC_Read_Cal(ADC_MemMapPtr adcmap, tADC_Cal_Blk *blk)
+{
+    blk->OFS  = ADC_OFS_REG(adcmap);
+    blk->PG   = ADC_PG_REG(adcmap);
+    blk->MG   = ADC_MG_REG(adcmap);
+    blk->CLPD = ADC_CLPD_REG(adcmap);
+    blk->CLPS = ADC_CLPS_REG(adcmap);
+    blk->CLP4 = ADC_CLP4_REG(adcmap);
+    blk->CLP3 = ADC_CLP3_REG(adcmap);
+    blk->CLP2 = ADC_CLP2_REG(adcmap);
+    blk->CLP1 = ADC_CLP1_REG(adcmap);
+    blk->CLP0 = ADC_CLP0_REG(adcmap);
+    blk->CLMD = ADC_CLMD_REG(adcmap);
+    blk->CLMS = ADC_CLMS_REG(adcmap);
+    blk->CLM4 = ADC_CLM4_REG(adcmap);
+    blk->CLM3 = ADC_CLM3_REG(adcmap);
+    blk->CLM2 = ADC_CLM2_REG(adcmap);
+    blk->CLM1 = ADC_CLM1_REG(adcmap);
+    blk->CLM0 = ADC_CLM0_REG(adcmap);
+
+}
+
+
+void TFC_InitADC0()
+{
+    tADC_Config Master_Adc0_Config;
+
+
+    SIM->SCGC6 |= (SIM_SCGC6_ADC0_MASK);
+
+    //Lets calibrate the ADC. 1st setup how the channel will be used.
+
+
+    Master_Adc0_Config.CONFIG1 = ADLPC_NORMAL           //No low power mode
+                                 | ADC_CFG1_ADIV(ADIV_4) //divide input by 4
+                                 | ADLSMP_LONG           //long sample time
+                                 | ADC_CFG1_MODE(MODE_12)//single ended 8-bit conversion
+                                 | ADC_CFG1_ADICLK(ADICLK_BUS);
+
+    Master_Adc0_Config.CONFIG2 = MUXSEL_ADCA // select the A side of the ADC channel.
+                                 | ADACKEN_DISABLED
+                                 | ADHSC_HISPEED
+                                 | ADC_CFG2_ADLSTS(ADLSTS_2);//Extra long sample Time (20 extra clocks)
+
+
+    Master_Adc0_Config.COMPARE1 = 00000; // Comparators don't matter for calibration
+    Master_Adc0_Config.COMPARE1 = 0xFFFF;
+
+    Master_Adc0_Config.STATUS2  = ADTRG_HW //hardware triggers for calibration
+                                  | ACFE_DISABLED //disable comparator
+                                  | ACFGT_GREATER
+                                  | ACREN_ENABLED
+                                  | DMAEN_DISABLED //Disable DMA
+                                  | ADC_SC2_REFSEL(REFSEL_EXT); //External Reference
+
+    Master_Adc0_Config.STATUS3 = CAL_OFF
+                                 | ADCO_SINGLE
+                                 | AVGE_ENABLED;
+                              //   | ADC_SC3_AVGS(AVGS_4);
+
+    Master_Adc0_Config.PGA =     0; // Disable the PGA
+
+
+    // Configure ADC as it will be used, but because ADC_SC1_ADCH is 31,
+    // the ADC will be inactive.  Channel 31 is just disable function.
+    // There really is no channel 31.
+
+    Master_Adc0_Config.STATUS1A = AIEN_ON | DIFF_SINGLE | ADC_SC1_ADCH(31);
+
+
+    ADC_Config_Alt(ADC0_BASE_PTR, &Master_Adc0_Config);  // config ADC
+
+    // Calibrate the ADC in the configuration in which it will be used:
+    ADC_Cal(ADC0_BASE_PTR);                    // do the calibration
+
+
+    Master_Adc0_Config.STATUS2  = ACFE_DISABLED //disable comparator
+                                  | ACFGT_GREATER
+                                  | ACREN_ENABLED
+                                  | DMAEN_DISABLED //Disable DMA
+                                  | ADC_SC2_REFSEL(REFSEL_EXT); //External Reference
+
+    Master_Adc0_Config.STATUS3 = CAL_OFF
+                                 | ADCO_SINGLE;
+
+
+
+    ADC_Config_Alt(ADC0_BASE_PTR, &Master_Adc0_Config);
+}
+
+
+void TFC_InitADC_System()
+{
+
+    TFC_InitADC0();
+
+
+    //All Adc processing of the Pots and linescan will be done in the ADC0 IRQ!
+    //A state machine will scan through the channels.
+    //This is done to automate the linescan capture on Channel 0 to ensure that timing is very even
+    CurrentADC_State =  ADC_STATE_INIT;
+
+    //The pump will be primed with the TPM1 interrupt.  upon timeout/interrupt it will set the SI signal high
+    //for the camera and then start the conversions for the pots.
+
+   // NVIC_SetVector(ADC0_IRQn,(uint32_t)ADC0_Handler);
+    NVIC_EnableIRQ(ADC0_IRQn);
+
+}
+
+extern "C" void ADC0_IRQHandler()
+{
+    uint8_t Junk;
+
+    switch(CurrentADC_State) {
+        default:
+            Junk =  ADC0->R[0];
+            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;
+
+            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;
+
+            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<TAOS_CLK_COUNT/2; Junk++) {
+            }
+
+            TAOS_SI_LOW;
+
+
+            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;
+
+            break;
+
+        case ADC_STATE_CAPTURE_LINE_SCAN:
+
+            if(CurrentLineScanPixel<128) {
+                if(CurrentLineScanChannel == 0) {
+                    LineScanImage0WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0];
+                    ADC0->SC1[0]  =  TFC_LINESCAN1_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+                    CurrentLineScanChannel = 1;
+
+                } else {
+                    LineScanImage1WorkingBuffer[CurrentLineScanPixel] = ADC0->R[0];
+                    ADC0->SC1[0]  =  TFC_LINESCAN0_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+                    CurrentLineScanChannel = 0;
+                    CurrentLineScanPixel++;
+
+                    TAOS_CLK_LOW;
+                    for(Junk = 0; Junk<TAOS_CLK_COUNT/2; Junk++) {
+                    }
+                    TAOS_CLK_HIGH;
+
+                }
+
+            } else {
+                // done with the capture sequence.  we can wait for the PIT0 IRQ to restart
+
+                TAOS_CLK_HIGH;
+
+                for(Junk = 0; Junk<TAOS_CLK_COUNT/2; Junk++) {
+                }
+
+                TAOS_CLK_LOW;
+                CurrentADC_State = ADC_STATE_INIT;
+
+                //swap the buffer
+
+                if(LineScanWorkingBuffer == 0) {
+                    LineScanWorkingBuffer = 1;
+
+                    LineScanImage0WorkingBuffer = &LineScanImage0Buffer[1][0];
+                    LineScanImage1WorkingBuffer = &LineScanImage1Buffer[1][0];
+
+                    TFC_LineScanImage0 = &LineScanImage0Buffer[0][0];
+                    TFC_LineScanImage1 = &LineScanImage1Buffer[0][0];
+                } else {
+                    LineScanWorkingBuffer = 0;
+                    LineScanImage0WorkingBuffer = &LineScanImage0Buffer[0][0];
+                    LineScanImage1WorkingBuffer = &LineScanImage1Buffer[0][0];
+
+                    TFC_LineScanImage0  = &LineScanImage0Buffer[1][0];
+                    TFC_LineScanImage1  = &LineScanImage1Buffer[1][0];
+                }
+
+                TFC_LineScanImageReady++;
+            }
+
+            break;
+    }
+
+}
+
+void TFC_InitLineScanCamera()
+{
+    SIM->SCGC5 |=   SIM_SCGC5_PORTE_MASK | SIM_SCGC5_PORTD_MASK; //Make sure the clock is enabled for PORTE;
+    PORTE->PCR[1] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;   //Enable GPIO on on the pin for the CLOCK Signal
+    PORTD->PCR[7] = PORT_PCR_MUX(1) | PORT_PCR_DSE_MASK;   //Enable GPIO on on the pin for SI signal
+
+    PORTD->PCR[5] = PORT_PCR_MUX(0); //Make sure AO signal goes to an analog input
+    PORTD->PCR[6] = PORT_PCR_MUX(0); //Make sure AO signal goes to an analog input
+
+    //Make sure the Clock and SI pins are outputs
+    PTD->PDDR |= (1<<7);
+    PTE->PDDR |= (1<<1);
+
+    TAOS_CLK_LOW;
+    TAOS_SI_LOW;
+
+    LineScanWorkingBuffer = 0;
+
+    LineScanImage0WorkingBuffer = &LineScanImage0Buffer[LineScanWorkingBuffer][0];
+    LineScanImage1WorkingBuffer = &LineScanImage1Buffer[LineScanWorkingBuffer][0];
+
+    TFC_LineScanImage0 = &LineScanImage0Buffer[1][0];
+    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.
+*/
+void TFC_InitMotorPWM(float SwitchingFrequency)
+{
+    //Clock Setup for the TPM requires a couple steps.
+
+    //1st,  set the clock mux
+    //See Page 124 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012
+    SIM->SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;// We Want MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012)
+    SIM->SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK);
+    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)
+
+
+    //Enable the Clock to the FTM0 Module
+    //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012
+    SIM->SCGC6 |= SIM_SCGC6_TPM0_MASK;
+
+    //The TPM Module has Clock.  Now set up the peripheral
+
+    //Blow away the control registers to ensure that the counter is not running
+    TPM0->SC = 0;
+    TPM0->CONF = 0;
+
+    //While the counter is disabled we can setup the prescaler
+
+    TPM0->SC = TPM_SC_PS(FTM0_CLK_PRESCALE);
+
+    //Setup the mod register to get the correct PWM Period
+
+    TPM0->MOD = (uint32_t)((float)(FTM0_CLOCK/(1<<FTM0_CLK_PRESCALE))/SwitchingFrequency);
+
+    //Setup Channels 0,1,2,3
+    TPM0->CONTROLS[0].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
+    TPM0->CONTROLS[1].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSA_MASK; // invert the second PWM signal for a complimentary output;
+    TPM0->CONTROLS[2].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
+    TPM0->CONTROLS[3].CnSC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSA_MASK; // invert the second PWM signal for a complimentary output;
+
+    //Enable the Counter
+
+    //Set the Default duty cycle to 50% duty cycle
+    TFC_SetMotorPWM(0.0,0.0);
+
+    //Enable the TPM COunter
+    TPM0->SC |= TPM_SC_CMOD(1);
+
+    //Enable the FTM functions on the the port
+    PORTC->PCR[1] = PORT_PCR_MUX(4);
+    PORTC->PCR[2] = PORT_PCR_MUX(4);
+    PORTC->PCR[3] = PORT_PCR_MUX(4);
+    PORTC->PCR[4] = PORT_PCR_MUX(4);
+
+}
+
+void TFC_SetMotorPWM(float MotorA , float MotorB)
+{
+    if(MotorA>1.0)
+        MotorA = 1.0;
+    else if(MotorA<-1.0)
+        MotorA = -1.0;
+
+    if(MotorB>1.0)
+        MotorB = 1.0;
+    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[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;
+
+}
+
+//Pot Reading is Scaled to return a value of -1.0 to 1.0
+float TFC_ReadPot(uint8_t Channel)
+{
+    if(Channel == 0)
+        return ((float)PotADC_Value[0]/-((float)ADC_MAX_CODE/2.0))+1.0;
+    else
+        return ((float)PotADC_Value[1]/-((float)ADC_MAX_CODE/2.0))+1.0;
+}
+
+float TFC_ReadBatteryVoltage()
+{
+    return (((float)BatSenseADC_Value/(float)(ADC_MAX_CODE)) * 3.0);// * ((47000.0+10000.0)/10000.0);
+}
+
+
+void TFC_SetBatteryLED_Level(uint8_t BattLevel)
+{
+    switch(BattLevel)
+    {
+        default:
+        case 0:
+            TFC_BAT_LED0_OFF;
+            TFC_BAT_LED1_OFF; 
+            TFC_BAT_LED2_OFF; 
+            TFC_BAT_LED3_OFF;
+        break;
+    
+        case 1:
+            TFC_BAT_LED0_ON;
+            TFC_BAT_LED1_OFF; 
+            TFC_BAT_LED2_OFF; 
+            TFC_BAT_LED3_OFF;
+        break;
+        
+        case 2:
+            TFC_BAT_LED0_ON;
+            TFC_BAT_LED1_ON; 
+            TFC_BAT_LED2_OFF; 
+            TFC_BAT_LED3_OFF;
+        break;
+        
+        case 3:
+            TFC_BAT_LED0_ON;
+            TFC_BAT_LED1_ON; 
+            TFC_BAT_LED2_ON; 
+            TFC_BAT_LED3_OFF;
+        break;
+        
+        case 4:
+            TFC_BAT_LED0_ON;
+            TFC_BAT_LED1_ON; 
+            TFC_BAT_LED2_ON; 
+            TFC_BAT_LED3_ON;
+        break;
+        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TFC.h	Wed Jul 02 03:23:07 2014 +0000
@@ -0,0 +1,252 @@
+
+#include "mbed.h"
+
+/** @file test.h*/
+
+/**
+ * @defgroup FRDM-TFC_API FRDM-TFC_API
+ *
+ * @{
+ */
+
+
+/**
+
+@addtogroup FRDM-TFC_API
+
+@{
+
+Resources used by the TFC Library\n
+
+I/O:\n
+-------------------------------------------------------------------------------------------------\n
+
+    PTB0   (Servo Channel 0 - TPM1)\n
+    PTB1   (Servo Channel 1 - TPM1)\n
+\n
+    PTB8   (Battery LED0)\n
+    PTB9   (Battery LED1)\n
+    PTB10  (Battery LED2)\n
+    PTB11  (Battery LED3)\n
+\n
+    PTD7   (Camera SI)\n
+    PTE0   (Camera CLK)\n
+    PTD5   (Camera A0  - ADC_SE6b)\n
+    PTD6   (Camera A1 - ADC_SE7b)\n
+\n
+    PTE2    DIP Switch 0\n
+    PTE3    DIP Switch 1\n
+    PTE4    DIP Switch 2\n
+    PTE5    DIP Switch 3\n
+
+    PTC13   Pushbutton SW1\n
+    PTC17   Pushbutton SW2\n
+
+    PTC3    H-Bridge A - 1 FTM0_CH3\n
+    PTC4    H-Bridge A - 2 FTM0_CH4\n
+    PTC1    H-Bridge B - 1 FTM0_CH1\n
+    PTC2    H-Bridge B - 2 FTM0_CH2\n
+
+    PTE21   H-Bridge Enable\n
+    PTE20   H-Bridge Fault\n
+
+    PTE23   H-Bridge A - IFB\n
+    PTE22   H-Bridge B - IFB\n
+
+    }
+*/
+
+
+
+#ifndef _TFC_H
+#define _TFC_H
+
+#define TFC_HBRIDGE_EN_LOC          (uint32_t)(1<<21)
+#define TFC_HBRIDGE_FAULT_LOC       (uint32_t)(1<<20)
+
+#define TFC_HBRIDGE_ENABLE          PTE->PSOR = TFC_HBRIDGE_EN_LOC
+#define TFC_HBRIDGE_DISABLE         PTE->PCOR = TFC_HBRIDGE_EN_LOC
+
+#define TFC_DIP_SWITCH0_LOC         ((uint32_t)(1<<2))
+#define TFC_DIP_SWITCH1_LOC         ((uint32_t)(1<<3))
+#define TFC_DIP_SWITCH2_LOC         ((uint32_t)(1<<4))
+#define TFC_DIP_SWITCH3_LOC         ((uint32_t)(1<<5))
+
+#define TFC_PUSH_BUTT0N0_LOC        ((uint32_t)(1<<13))
+#define TFC_PUSH_BUTT0N1_LOC        ((uint32_t)(1<<17))
+
+#define TFC_BAT_LED0_LOC            ((uint32_t)(1<<11))
+#define TFC_BAT_LED1_LOC            ((uint32_t)(1<<10))
+#define TFC_BAT_LED2_LOC            ((uint32_t)(1<<9))
+#define TFC_BAT_LED3_LOC            ((uint32_t)(1<<8))
+
+#define TAOS_CLK_HIGH  PTE->PSOR = (1<<1)
+#define TAOS_CLK_LOW   PTE->PCOR = (1<<1)
+#define TAOS_SI_HIGH   PTD->PSOR = (1<<7)
+#define TAOS_SI_LOW    PTD->PCOR = (1<<7)
+
+
+/**
+
+@addtogroup FRDM-TFC_API
+@{
+*/
+
+/**Macro to turn on LED 0 in the battery indicator array*/
+#define TFC_BAT_LED0_ON             PTB->PSOR = TFC_BAT_LED0_LOC
+/** Macro to turn on LED 1 in the battery indicator array*/
+#define TFC_BAT_LED1_ON             PTB->PSOR = TFC_BAT_LED1_LOC
+/** Macro to turn on LED 2 in the battery indicator array*/
+#define TFC_BAT_LED2_ON             PTB->PSOR = TFC_BAT_LED2_LOC
+/** Macro to turn on LED 3 in the battery indicator array*/
+#define TFC_BAT_LED3_ON             PTB->PSOR = TFC_BAT_LED3_LOC
+
+
+/** Macro to turn off LED 0 in the battery indicator array*/
+#define TFC_BAT_LED0_OFF            PTB->PCOR = TFC_BAT_LED0_LOC
+/** Macro to turn off LED 1 in the battery indicator array*/
+#define TFC_BAT_LED1_OFF            PTB->PCOR = TFC_BAT_LED1_LOC
+/** Macro to turn off LED 2 in the battery indicator array*/
+#define TFC_BAT_LED2_OFF            PTB->PCOR = TFC_BAT_LED2_LOC
+/** Macro to turn off LED 3 in the battery indicator array*/
+#define TFC_BAT_LED3_OFF            PTB->PCOR = TFC_BAT_LED3_LOC
+
+
+/** Macro to toggle LED 0 in the battery indicator array*/
+#define TFC_BAT_LED0_TOGGLE         PTB->PTOR = TFC_BAT_LED0_LOC
+/** Macro to toggle LED 1 in the battery indicator array*/
+#define TFC_BAT_LED1_TOGGLE         PTB->PTOR = TFC_BAT_LED1_LOC
+/** Macro to toggle LED 2 in the battery indicator array*/
+#define TFC_BAT_LED2_TOGGLE         PTB->PTOR = TFC_BAT_LED2_LOC
+/** Macro to toggle LED 3 in the battery indicator array*/
+#define TFC_BAT_LED3_TOGGLE         PTB->PTOR = TFC_BAT_LED3_LOC
+
+
+/** Macro to read the state of the pushbutton SW1*/
+#define TFC_PUSH_BUTTON_0_PRESSED   ((PTC->PDIR&TFC_PUSH_BUTT0N0_LOC)>0)
+/** Macro to read the state of the pushbutton SW1*/
+#define TFC_PUSH_BUTTON_1_PRESSED   ((PTC->PDIR&TFC_PUSH_BUTT0N1_LOC)>0)
+
+/** Macro to read the state of switch 0 in the 4 position DIP switch*/
+#define TFC_DIP_SWITCH_0_ON         ((TFC_GetDIP_Switch()&0x01)>0)
+
+/** Macro to read the state of switch 1 in the 4 position DIP switch*/
+#define TFC_DIP_SWITCH_1_ON         ((TFC_GetDIP_Switch()&0x02)>0)
+
+/** Macro to read the state of switch 2 in the 4 position DIP switch*/
+#define TFC_DIP_SWITCH_2_ON         ((TFC_GetDIP_Switch()&0x04)>0)
+
+/** Macro to read the state of switch 3 in the 4 position DIP switch*/
+#define TFC_DIP_SWITCH_3_ON         ((TFC_GetDIP_Switch()&0x08)>0)
+
+
+/** Initialized the TFC API.   Call before using any other API calls.
+*
+*/
+void TFC_Init();
+
+/** ServoTicker will increment once every servo cycle.
+* It can be used to synchronize events to the start of a servo cycle. ServoTicker is a volatile uint32_t and is updated in the TPM1 overlflow interrupt.  This means you will see ServoTicker increment on the rising edge of the servo PWM signal
+*
+*/
+ extern volatile uint32_t TFC_ServoTicker;
+
+
+/** Gets the state of the 4-positiomn DIP switch on the FRDM-TFC
+*
+*  @returns The lower 4-bits of the return value map to the 4-bits of the DIP switch
+*/
+uint8_t TFC_GetDIP_Switch();
+
+
+/** Reads the state of the pushbuttons (SW1, SW2)  on the FRDM-TFC
+*  @param Index Selects the pushbutton (0 for SW1 and 1 for SW2)
+*  @returns A non-zero value if the button is pushed
+*/
+uint8_t TFC_ReadPushButton(uint8_t Index);
+
+
+/** Controls the 4 battery level LEDs on the FRDM-TFC boards.
+*
+*  @param Value  The lower 4-bits of the parameter maps to the 4 LEDs.
+*/
+void TFC_SetBatteryLED(uint8_t Value);
+
+
+/** Sets the servo channels
+*
+*  @param ServoNumber  Which servo channel on the FRDM-TFC to use (0 or 1).  0 is the default channel for steering
+*  @param Position     Angle setting for servo in a normalized (-1.0 to 1.0) form.   The range of the servo can be changed with the InitServos function.
+*                       This is called in the TFC constructor with some useful default values-->  20mSec period,  0.5mS min and 2.0mSec max.  you may need to adjust these for your own particular setup.
+*/
+void TFC_SetServo(uint8_t ServoNumber, float Position);
+
+/** Initializes TPM for the servoes.  It also sets the max and min ranges
+*
+*  @param ServoPulseWidthMin    Minimum pulse width (in seconds) for the servo.   The value of -1.0 in SetServo is mapped to this pulse width.  I.E.  .001
+*  @param ServoPulseWidthMax    Maximum pulse width (in seconds) for the servo.   The value of +1.0 in SetServo is mapped to this pulse width.  I.E.  .002
+*  @param ServoPeriod           Period of the servo pulses (in seconds).  I.e.  .020 for 20mSec
+*/
+
+void TFC_InitServos(float ServoPulseWidthMin, float ServoPulseWidthMax, float ServoPeriod);
+
+
+/** 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.
+*/
+void TFC_InitMotorPWM(float SwitchingFrequency);
+
+/** Sets the PWM value for each motor.
+*
+*  @param MotorA    The PWM value for HBridgeA. The value is normalized to the floating point range of -1.0 to +1.0.    -1.0 is 0% (Full Reverse on the H-Bridge) and 1.0 is 100% (Full Forward on the H-Bridge)
+*  @param MotorB    The PWM value for HBridgeB. The value is normalized to the floating point range of -1.0 to +1.0.    -1.0 is 0% (Full Reverse on the H-Bridge) and 1.0 is 100% (Full Forward on the H-Bridge)
+*/
+void TFC_SetMotorPWM(float MotorA ,float MotorB);
+
+/** Reads the potentiometers
+*
+*  @param Channel   Selects which pot is read.   I.e.  0 for POT0 or 1 for POT1
+*  @returns    Pot value from -1.0 to 1.0
+*/
+float TFC_ReadPot(uint8_t Channel);
+
+/** Gets the current battery voltage
+*
+*  @returns    Battery voltage in floating point form.
+*/
+float TFC_ReadBatteryVoltage();
+
+
+
+/** Sets the Battery level indiciate
+*
+*  @param BattLevel   A number betwween 0 and 4.   This will light the bar from left to right with the specifified number of segments.
+*
+*/
+void TFC_SetBatteryLED_Level(uint8_t BattLevel);
+
+
+/** 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.   
+*
+*/
+
+extern volatile uint16_t * TFC_LineScanImage0;
+extern volatile uint16_t * TFC_LineScanImage1;
+
+
+/** This flag will increment when a new frame is ready.  Check for a non zero value (and reset to zero!) when you want to read the camera(s)
+*
+*/
+
+extern volatile uint8_t TFC_LineScanImageReady;
+
+
+
+
+
+/** @} */
+
+
+#endif

--- a/bx-adc.cpp	Mon Jun 30 07:01:58 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,94 +0,0 @@
-#include "bx-adc.h"
-#include "clk_freqs.h"
- 
-#define MAX_FADC            6000000
-#define CHANNELS_A_SHIFT    5
- 
-FastAnalogIn::FastAnalogIn(PinName pin, bool enabled)
-{
-    ADCnumber = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
-    if (ADCnumber == (ADCName)NC) {
-        error("ADC pin mapping failed");
-    }
- 
-    SIM->SCGC6 |= SIM_SCGC6_ADC0_MASK;
- //-----------------------------------------------------
-    uint32_t port = (uint32_t)pin >> PORT_SHIFT;
-    SIM->SCGC5 |= 1 << (SIM_SCGC5_PORTA_SHIFT + port);
- 
-    uint32_t cfg2_muxsel = ADC_CFG2_MUXSEL_MASK;
-    if (ADCnumber & (1 << CHANNELS_A_SHIFT)) {
-        cfg2_muxsel = 0;
-    }
- //------------------------------------------------------    
-    // bus clk
-    uint32_t PCLK = bus_frequency();
-    uint32_t clkdiv;
-    for (clkdiv = 0; clkdiv < 4; clkdiv++) {
-        if ((PCLK >> clkdiv) <= MAX_FADC)
-            break;
-    }
-    if (clkdiv == 4)                    //Set max div
-        clkdiv = 0x7;
- 
-    ADC0->SC1[1] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
- 
-    ADC0->CFG1 = ADC_CFG1_ADIV(clkdiv & 0x3)    // Clock Divide Select: (Input Clock)/8
-               | ADC_CFG1_MODE(3)               // (16)bits Resolution
-               | ADC_CFG1_ADICLK(clkdiv >> 2);  // Input Clock: (Bus Clock)/2
- 
-    ADC0->CFG2 = cfg2_muxsel            // ADxxb or ADxxa channels
-               | ADC_CFG2_ADACKEN_MASK  // Asynchronous Clock Output Enable
-               | ADC_CFG2_ADHSC_MASK;   // High-Speed Configuration
- 
-    ADC0->SC2 = ADC_SC2_REFSEL(0);      // Default Voltage Reference
- 
-    pinmap_pinout(pin, PinMap_ADC);
- 
-    //Enable channel
-    running = false;
-    enable(enabled);
-}
- 
-void FastAnalogIn::enable(bool enabled)
-{
-    //If currently not running
-    if (!running) {
-        if (enabled) {
-            //Enable the ADC channel
-            ADC0->SC3 |= ADC_SC3_ADCO_MASK;       // Enable continuous conversion
-            ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));  //Start conversion
-            running = true;
-        } else
-            disable();
-    }
-}
- 
-void FastAnalogIn::disable( void )
-{
-    //If currently running
-    if (running) {
-        ADC0->SC3 &= ~ADC_SC3_ADCO_MASK;      // Disable continuous conversion
-    }
-    running = false;
-}
- 
-uint16_t FastAnalogIn::read_u16()
-{
-    if (!running)
-    {
-        // start conversion
-        ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
-        // Wait Conversion Complete
-        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
-    }
-    if(running && ((ADC0->SC1[0]&ADC_SC1_ADCH_MASK) != (ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT)))))
-    {
-        running = false;
-        enable();
-        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
-    }
-    // Return value
-    return (uint16_t)ADC0->R[0];
-}
- 
\ No newline at end of file

--- a/bx-adc.h	Mon Jun 30 07:01:58 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,117 +0,0 @@
-#include "mbed.h"
-#include "pinmap.h"
- 
-
- 
- /** A class similar to AnalogIn, only faster, for LPC1768, LPC408X and KLxx
- *
- * AnalogIn does a single conversion when you read a value (actually several conversions and it takes the median of that).
- * This library runns the ADC conversion automatically in the background.
- * When read is called, it immediatly returns the last sampled value.
- *
- * LPC1768 / LPC4088
- * Using more ADC pins in continuous mode will decrease the conversion rate (LPC1768:200kHz/LPC4088:400kHz).
- * If you need to sample one pin very fast and sometimes also need to do AD conversions on another pin,
- * you can disable the continuous conversion on that ADC channel and still read its value.
- *
- * KLXX
- * Multiple Fast instances can be declared of which only ONE can be continuous (all others must be non-continuous).
- *
- * When continuous conversion is disabled, a read will block until the conversion is complete
- * (much like the regular AnalogIn library does).
- * Each ADC channel can be enabled/disabled separately.
- *
- * IMPORTANT : It does not play nicely with regular AnalogIn objects, so either use this library or AnalogIn, not both at the same time!!
- *
- * Example for the KLxx processors:
- * @code
- * // Print messages when the AnalogIn is greater than 50%
- *
- * #include "mbed.h"
- *
- * FastAnalogIn temperature(PTC2); //Fast continuous sampling on PTC2
- * FastAnalogIn speed(PTB3, 0);    //Fast non-continuous sampling on PTB3
- *
- * int main() {
- *     while(1) {
- *         if(temperature > 0.5) {
- *             printf("Too hot! (%f) at speed %f", temperature.read(), speed.read());
- *         }
- *     }
- * }
- * @endcode
- * Example for the LPC1768 processor:
- * @code
- * // Print messages when the AnalogIn is greater than 50%
- *
- * #include "mbed.h"
- *
- * FastAnalogIn temperature(p20);
- *
- * int main() {
- *     while(1) {
- *         if(temperature > 0.5) {
- *             printf("Too hot! (%f)", temperature.read());
- *         }
- *     }
- * }
- * @endcode
-*/
-class FastAnalogIn {
- 
-public:
-     /** Create a FastAnalogIn, connected to the specified pin
-     *
-     * @param pin AnalogIn pin to connect to
-     * @param enabled Enable the ADC channel (default = true)
-     */
-    FastAnalogIn( PinName pin, bool enabled = true );
-    
-    ~FastAnalogIn( void )
-    {
-        disable();
-    }
-    
-    /** Enable the ADC channel
-    *
-    * @param enabled Bool that is true for enable, false is equivalent to calling disable
-    */
-    void enable(bool enabled = true);
-    
-    /** Disable the ADC channel
-    *
-    * Disabling unused channels speeds up conversion in used channels. 
-    * When disabled you can still call read, that will do a single conversion (actually two since the first one always returns 0 for unknown reason).
-    * Then the function blocks until the value is read. This is handy when you sometimes needs a single conversion besides the automatic conversion
-    */
-    void disable( void );
-    
-    /** Returns the raw value
-    *
-    * @param return Unsigned integer with converted value
-    */
-    unsigned short read_u16( void );
-    
-    /** Returns the scaled value
-    *
-    * @param return Float with scaled converted value to 0.0-1.0
-    */
-    float read( void )
-    {
-        unsigned short value = read_u16();
-        return (float)value * (1.0f/65535.0f);
-    }
-    
-    /** An operator shorthand for read()
-    */
-    operator float() {
-        return read();
-    }
- 
-    
-private:
-    bool running;    
-    char ADCnumber;
-    uint32_t *datareg;
-};
- 

--- a/camera_api.cpp	Mon Jun 30 07:01:58 2014 +0000
+++ b/camera_api.cpp	Wed Jul 02 03:23:07 2014 +0000
@@ -1,18 +1,21 @@
 #include "mbed.h"
 #include "camera_api.h"
-
+#include "TFC.h"
 #define clk 2  //ms
+#define task_ma_time
+#ifdef task_ma_time
+DigitalOut cam_p(PTE5); 
+#endif
 
 BX_camera::BX_camera(int p)
 {
-
+ 
     line_CamR = new FastAnalogIn(PTD5);
     line_CamL=   new FastAnalogIn(PTD6,0);
     cam_clk=new DigitalOut(PTE1);
     si=new DigitalOut(PTD7);
     padding = p;
 }
-
 int BX_camera::black_centerR(void)
 {
 
@@ -128,95 +131,134 @@
 
 
 
+ 
 void BX_camera::read(void)
 {
-
+ 
     w_f_vL=0x0000;
     b_f_vL=0xffff;
-
+ 
     w_f_vR=0x0000;
     b_f_vR=0xffff;
-
-
-
-
+ 
+ 
+     line_CamR->enable();
+ 
     *si=1;
     *cam_clk=1;
-
+ 
     wait_us(30);   // tune here
     *si=0;
     *cam_clk=0;
-
-
-
-    line_CamR->enable();
-    line_CamL->enable();
-
-
+ 
+ 
+   
+    
+ 
+ 
     //input 128 //both
-
+ 
     for(int i=0; i<128; i++) {
         *cam_clk=1;
         wait_us(5);
-
-
+ 
+ 
         line_imageR[i]=line_CamR->read_u16();
-        line_imageL[i]=line_CamL->read_u16();
-
+  
+      
+      
+       
+ 
         //  big small
         if(line_imageR[i] > w_f_vR)
             w_f_vR=line_imageR[i];
         else if(line_imageR[i] < b_f_vR )
             b_f_vR = line_imageR[i];
-
-
+ 
+ 
+ 
+ 
+ 
+        *cam_clk=0;
+        wait_us(5);
+ 
+ 
+    }
+   line_CamR->disable();
+ /*
+  *si=1;
+    *cam_clk=1;
+ 
+    wait_us(30);   // tune here
+    *si=0;
+    *cam_clk=0;
+ 
+ 
+    
+    cam_p=1;
+    line_CamL->enable();
+     for(int i=0; i<128; i++) {
+        *cam_clk=1;
+        wait_us(5);
+ 
+ 
+    
+  
+      
+      
+        line_imageL[i]=line_CamL->read_u16();
+ 
+        //  big small
+     
+ 
         if(line_imageL[i] > w_f_vL)
             w_f_vL=line_imageL[i];
         else if(line_imageL[i] < b_f_vL )
             b_f_vL = line_imageL[i];
-
-
-
-
+ 
+ 
+ 
+ 
         *cam_clk=0;
         wait_us(5);
-
-
+ 
+ 
     }
-
-
-    line_CamR->enable();
-    line_CamL->enable();
-
-
+    
+    
+   
+  
+    line_CamL->disable();
+     cam_p=0; 
+ */
     //filter L R   //may change
-
+ 
     for(int i=0; i<128; i++) {
-
-
+ 
+ 
         if( (line_imageR[i]-b_f_vR) < (w_f_vR - line_imageR[i] )    )
             sign_line_imageR[i]=' ';
         else
             sign_line_imageR[i]='O';
-
-
+ 
+ 
         if( (line_imageL[i]-b_f_vL) < (w_f_vL - line_imageL[i] )    )
             sign_line_imageL[i]=' ';
         else
             sign_line_imageL[i]='O';
-
-
-
-
-
+ 
+ 
+ 
+ 
+ 
         if(i==0) {
             sign_line_imageR[i]='X';
             sign_line_imageL[i]='X';
         }
-
-
+ 
+ 
     }
-
-
-
+ 
+ 
+ 
 }
\ No newline at end of file

--- a/camera_api.h	Mon Jun 30 07:01:58 2014 +0000
+++ b/camera_api.h	Wed Jul 02 03:23:07 2014 +0000
@@ -1,9 +1,9 @@
-#include "bx-adc.h"
+#include "BX-adc.h"
 #include "mbed.h"
-
-
+ 
+ 
 //cause same si and clk  camR ,camL are synchronous
-
+ 
 class BX_camera{
     
   public:
@@ -47,7 +47,7 @@
     
     
           int padding;
-
+ 
     
     
     

--- a/main.cpp	Mon Jun 30 07:01:58 2014 +0000
+++ b/main.cpp	Wed Jul 02 03:23:07 2014 +0000
@@ -9,11 +9,11 @@
 #include "Stack.h"
 
 #define Debug_cam_uart
-#define L_eye
-//#define R_eye
+//#define L_eye
+#define R_eye
 #define motor_on 
 #define Pcontroller
-#define task_ma_time
+//#define task_ma_time
 
 
 #define R_target 64
@@ -21,9 +21,9 @@
 
 
 
-#define t_cam 2
+#define t_cam 6
 
-
+#define black_center 64
 
 
  Serial pc(USBTX, USBRX); // tx, rx
@@ -44,9 +44,10 @@
 PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
 
 #ifdef task_ma_time
-DigitalOut cam_p(PTD1); //cam       black
-DigitalOut servo_p(PTA13); //servo   coffee
-DigitalOut de_p(PTD3);   //   red
+DigitalOut cam_p(PTE5); //cam       black
+DigitalOut servo_p(PTE4); //servo   coffee
+DigitalOut idle_p (PTE3);
+//DigitalOut de_p(PTD3);   //   red
 #endif
 TSISensor tsi;
 
@@ -64,8 +65,7 @@
 
 
 
-static int b_r_c=64;
-static int b_l_c=64;
+
 static int pre_b_r_c;
 
 static double v_motor;
@@ -80,37 +80,52 @@
     while(true){
         #ifdef task_ma_time
         cam_p=1;
-        #endif   
+        #endif  
+        
            cam.read();
         
-     
-            b_r_c=cam.black_centerR();
-           // b_l_c=cam.black_centerL();
-           
-           points.push(b_r_c);           
-           
-           
+             
         #ifdef task_ma_time
         cam_p=0;
         #endif    
+        
+        int b_r_c,b_l_c;
+                
+      
+            b_r_c=cam.black_centerR();
+           // b_l_c=cam.black_centerL();
+      
+      //right
+          // printf("push :%d\r\n",b_r_c);     
+           points.push(b_r_c);           
+           
+      
            
            
             Thread::wait(t_cam);
-        
+      
         }
     
 }
 // function
+
+
+
+
+
+
+
+
 void de_thread(void const *args){
     
     while(1){
         
           
         #ifdef task_ma_time
-        de_p=0;
+        //de_p=0;
         #endif       
         
-        
+        cam.read();
     stdio_mutex.lock();    
     #ifdef Debug_cam_uart 
       #ifdef L_eye  
@@ -145,18 +160,20 @@
          pc.printf("\r\n Rcenter :%d Lcenter : %d servo: %f \r\n",cam.black_centerR(),cam.black_centerL(),v_servo);
     
          
+         pc.printf("stack n: %d",points.available());
+         
          stdio_mutex.unlock();
      
          
            
         #ifdef task_ma_time
-        de_p=1;
+        //de_p=1;
         #endif   
        
          
          
 #endif   
-             Thread::wait(1);
+             Thread::wait(10);
 
       
     }
@@ -175,30 +192,44 @@
         servo_p=1;
         #endif
         
-        int point;
-        
-        for(;points.available()!=0;){
-            points.pop(&point);
-        
+       int point;
+       int sum_e=0;
+       int n_point=0; 
+       n_point=points.available() ;
+       
+        for(int i=0;i<n_point;i++){
+             points.pop(&point);
         //algorithm
-        
+           
+             sum_e+=point-black_center;  
+     
         }
-             
+             printf("error :%d compute: %f\r\n",sum_e/n_point, cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target));
+        
+        
+        cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target);  
+       // pc.printf("expect :%d \r\n",black_center+sum_e/n_point );
+       // pc.printf("angle :%f", cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target)  );
+          servo.set_angle(         cam_to_M_ctrlr.compute(black_center+sum_e/n_point,R_target) );
+         
+        // pc.printf("dfdf");    
         //if(b_r_c!=-1)
-         v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);     
+        // v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);     
         //if(b_l_c!=-1)
         // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);     
             
         
              // v_servo=pot2.read();
-         servo.set_angle(v_servo);
+       
         
         #ifdef task_ma_time
         servo_p=0;
         #endif 
         
               
-         Thread::wait(20);    
+         Thread::wait(20);
+        
+             
         }    
     
     
@@ -241,14 +272,21 @@
   
     servo.set_angle(0.055);
     pc.baud(115200);
-   /*
-      while(1){
+   
+    
+   
+     //while(1);
+     
+   
+
+   
+  /*    while(1){
          
              if(tsi.readPercentage()>0.00011)
              break;
       }
-    */
-  
+    
+  */
     
     
     
@@ -256,15 +294,27 @@
    //   Thread thread(ctrl_thread);  
       Thread th_s(servo_thread);  
    //   Thread th_m(motor_thread);     
-    // Thread th_de(de_thread);  
+    // Thread th_de(de_thread);
+     
+     //Thread dddd(pin2_thread);  
      while(1){
          
+          #ifdef task_ma_time
+             idle_p=1;
+          #endif
+           
+           
+           
+          #ifdef task_ma_time
+             idle_p=0;
+          #endif
+           
            
      //idle
   //   stdio_mutex.lock();
     //    printf("L: %d  mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
     // stdio_mutex.unlock();     
-         // Thread::wait(2000);
+          Thread::wait(1000);
     
      }