Diff: TFC.cpp

Revision:

10:86c5fc9a53d9

Parent:

9:1c4fc3e224b5

Child:

11:787a9a24dbfb

--- a/TFC.cpp	Wed Feb 17 12:41:15 2016 +0000
+++ b/TFC.cpp	Wed Feb 17 20:42:32 2016 +0000
@@ -24,13 +24,17 @@
 #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_I_HBRIDGE_A           5
+#define ADC_STATE_CAPTURE_I_HBRIDGE_B           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_LINESCAN0_ADC_CHANNEL   6 // Mux B
+#define TFC_LINESCAN1_ADC_CHANNEL   7 // Mux B
+// RAJOUTER UN CHANNEL ICI
+#define TFC_I_HBRIDGE_0_SENSE_ADC_CHANNEL 7 // Mux A
+#define TFC_I_HBRIDGE_1_SENSE_ADC_CHANNEL 3 // Mux A
 
 
 #define ADC0_irq_no   57
@@ -350,6 +354,9 @@
 volatile uint8_t  CurrentLineScanChannel;
 volatile uint32_t TFC_ServoTicker;
 
+volatile uint16_t HBridge_0_current_value;
+volatile uint16_t HBridge_1_current_value;
+
 
 void TFC_SetServoDutyCycle(uint8_t ServoNumber, float DutyCycle);
 void TFC_InitLineScanCamera();
@@ -787,10 +794,28 @@
         case ADC_STATE_CAPTURE_POT_1:
 
             PotADC_Value[1] = ADC0->R[0];
+            
+            CurrentADC_State = ADC_STATE_CAPTURE_I_HBRIDGE_A; 
+            ADC0->SC1[0]  =  TFC_I_HBRIDGE_0_SENSE_ADC_CHANNEL | ADC_SC1_AIEN_MASK; // HBRIDGE channel
+            ADC0->CFG2  &= ~ADC_CFG2_MUXSEL_MASK; //Select the A side of the mux
+            break;
+            
+            
+        // NEW STATES
+        case ADC_STATE_CAPTURE_I_HBRIDGE_A:
+            // Read ADC value 
+            HBridge_0_current_value = ADC0->R[0];
+            // Change the ADC active channel 
+            ADC0->SC1[0]  =  TFC_I_HBRIDGE_1_SENSE_ADC_CHANNEL | ADC_SC1_AIEN_MASK;
+            // Update the state machine
+            CurrentADC_State = ADC_STATE_CAPTURE_I_HBRIDGE_B;
+            break;
+            
+        case ADC_STATE_CAPTURE_I_HBRIDGE_B:
+            HBridge_1_current_value = 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:
@@ -846,7 +871,7 @@
 
                 TAOS_CLK_LOW;
                 CurrentADC_State = ADC_STATE_INIT;
-
+                
                 //swap the buffer
 
                 if(LineScanWorkingBuffer == 0) {
@@ -993,7 +1018,10 @@
 
 float TFC_ReadMotorCurrent(uint8_t motorChannel)
 {
-    return 0.0;   
+    if(motorChannel)
+        return ((float)(HBridge_0_current_value)/(float)(ADC_MAX_CODE));//TODO : Coefficient ?
+    else
+        return ((float)(HBridge_1_current_value)/(float)(ADC_MAX_CODE));//TODO : Coefficient ?
 }