Eli Hughes
/
RobotPowerLogger-V2
For Terrance
Code/ADC.c@0:085749c8446f, 2012-06-13 (annotated)
- Committer:
- emh203
- Date:
- Wed Jun 13 15:10:06 2012 +0000
- Revision:
- 0:085749c8446f
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
emh203 | 0:085749c8446f | 1 | #include "System.h" |
emh203 | 0:085749c8446f | 2 | |
emh203 | 0:085749c8446f | 3 | volatile float BatteryVoltage = 0; |
emh203 | 0:085749c8446f | 4 | volatile float RobotBusVoltageHR = 0; |
emh203 | 0:085749c8446f | 5 | volatile float RobotBusCurrentHR = 0; |
emh203 | 0:085749c8446f | 6 | |
emh203 | 0:085749c8446f | 7 | float HighResRobotBusVoltageScalingFactor; |
emh203 | 0:085749c8446f | 8 | float HighResRobotBusVoltageOffset; |
emh203 | 0:085749c8446f | 9 | float HighResRobotBusCurrentScalingFactor; |
emh203 | 0:085749c8446f | 10 | |
emh203 | 0:085749c8446f | 11 | SIGNED_DWORD ExtADCRobotVoltage=0; |
emh203 | 0:085749c8446f | 12 | SIGNED_DWORD ExtADCRobotCurrent=0; |
emh203 | 0:085749c8446f | 13 | |
emh203 | 0:085749c8446f | 14 | AnalogIn BatteryChannel(p17); |
emh203 | 0:085749c8446f | 15 | AnalogIn RobotBusCurrentChannel(p20); |
emh203 | 0:085749c8446f | 16 | AnalogIn RobotBusVoltageChannel(p19); |
emh203 | 0:085749c8446f | 17 | AnalogIn SelfCurrentChannel(p18); |
emh203 | 0:085749c8446f | 18 | |
emh203 | 0:085749c8446f | 19 | Ticker BatteryCheck; |
emh203 | 0:085749c8446f | 20 | Ticker InputPowerCheck; |
emh203 | 0:085749c8446f | 21 | |
emh203 | 0:085749c8446f | 22 | SPI ADCSPI(p11, p12, p13); |
emh203 | 0:085749c8446f | 23 | DigitalOut ConversionStart(p14); |
emh203 | 0:085749c8446f | 24 | |
emh203 | 0:085749c8446f | 25 | void CheckBattery(); |
emh203 | 0:085749c8446f | 26 | void CheckInputPower(); |
emh203 | 0:085749c8446f | 27 | |
emh203 | 0:085749c8446f | 28 | SIGNED_DWORD VFilter[64]; |
emh203 | 0:085749c8446f | 29 | SIGNED_DWORD IFilter[64]; |
emh203 | 0:085749c8446f | 30 | WORD FilterIndex; |
emh203 | 0:085749c8446f | 31 | |
emh203 | 0:085749c8446f | 32 | SIGNED_DWORD VBattFilter[8]; |
emh203 | 0:085749c8446f | 33 | WORD VBattFilterIndex; |
emh203 | 0:085749c8446f | 34 | |
emh203 | 0:085749c8446f | 35 | DigitalOut PING(LED3); |
emh203 | 0:085749c8446f | 36 | DigitalOut PONG(LED4); |
emh203 | 0:085749c8446f | 37 | |
emh203 | 0:085749c8446f | 38 | volatile BOOL ADCDataRdy; |
emh203 | 0:085749c8446f | 39 | |
emh203 | 0:085749c8446f | 40 | |
emh203 | 0:085749c8446f | 41 | void InitADC() |
emh203 | 0:085749c8446f | 42 | { |
emh203 | 0:085749c8446f | 43 | HighResRobotBusVoltageScalingFactor = U1_VREF * ((R2 + R4)/(R4)) * VSCALE_ADJUST; |
emh203 | 0:085749c8446f | 44 | HighResRobotBusVoltageOffset = INPUT_DIODE_DROP*2; |
emh203 | 0:085749c8446f | 45 | HighResRobotBusCurrentScalingFactor = U1_VREF * ((R6+R7)/R7) ; |
emh203 | 0:085749c8446f | 46 | |
emh203 | 0:085749c8446f | 47 | ACS576_ISCALE = ACS576_ISCALE * ACS576_ISCALE_ADJUST; |
emh203 | 0:085749c8446f | 48 | |
emh203 | 0:085749c8446f | 49 | ADCSPI.format(16,0); |
emh203 | 0:085749c8446f | 50 | |
emh203 | 0:085749c8446f | 51 | ConversionStart = 0; |
emh203 | 0:085749c8446f | 52 | |
emh203 | 0:085749c8446f | 53 | BatteryCheck.attach(&CheckBattery,0.1); |
emh203 | 0:085749c8446f | 54 | InputPowerCheck.attach(&CheckInputPower,(1.0/(SAMPLE_RATE*16))); |
emh203 | 0:085749c8446f | 55 | } |
emh203 | 0:085749c8446f | 56 | |
emh203 | 0:085749c8446f | 57 | void CheckBattery() |
emh203 | 0:085749c8446f | 58 | { |
emh203 | 0:085749c8446f | 59 | int i; |
emh203 | 0:085749c8446f | 60 | SIGNED_DWORD TempSum = 0; |
emh203 | 0:085749c8446f | 61 | |
emh203 | 0:085749c8446f | 62 | VBattFilter[VBattFilterIndex++] = BatteryChannel.read_u16(); |
emh203 | 0:085749c8446f | 63 | VBattFilterIndex &= 0x7; |
emh203 | 0:085749c8446f | 64 | |
emh203 | 0:085749c8446f | 65 | for(i=0;i<8;i++) |
emh203 | 0:085749c8446f | 66 | { |
emh203 | 0:085749c8446f | 67 | TempSum += VBattFilter[i]; |
emh203 | 0:085749c8446f | 68 | } |
emh203 | 0:085749c8446f | 69 | |
emh203 | 0:085749c8446f | 70 | BatteryVoltage = (float)((TempSum>>3))/65535.0f * MBED_VREF * ((R17+R15)/R15); |
emh203 | 0:085749c8446f | 71 | } |
emh203 | 0:085749c8446f | 72 | |
emh203 | 0:085749c8446f | 73 | void CheckInputPower() |
emh203 | 0:085749c8446f | 74 | { |
emh203 | 0:085749c8446f | 75 | SIGNED_DWORD TempSumV,TempSumI; |
emh203 | 0:085749c8446f | 76 | int i; |
emh203 | 0:085749c8446f | 77 | |
emh203 | 0:085749c8446f | 78 | ConversionStart = 1; |
emh203 | 0:085749c8446f | 79 | VFilter[FilterIndex] = (SIGNED_WORD)(ADCSPI.write(0x0000)); |
emh203 | 0:085749c8446f | 80 | IFilter[FilterIndex] = (SIGNED_WORD)(ADCSPI.write(0x0000)); |
emh203 | 0:085749c8446f | 81 | FilterIndex++; |
emh203 | 0:085749c8446f | 82 | ConversionStart = 0; |
emh203 | 0:085749c8446f | 83 | |
emh203 | 0:085749c8446f | 84 | //oversample and average |
emh203 | 0:085749c8446f | 85 | if(FilterIndex ==16) |
emh203 | 0:085749c8446f | 86 | { |
emh203 | 0:085749c8446f | 87 | ADCDataRdy = TRUE; |
emh203 | 0:085749c8446f | 88 | FilterIndex = 0; |
emh203 | 0:085749c8446f | 89 | TempSumV=0; |
emh203 | 0:085749c8446f | 90 | TempSumI=0; |
emh203 | 0:085749c8446f | 91 | for(i=0;i<32;i++) |
emh203 | 0:085749c8446f | 92 | { |
emh203 | 0:085749c8446f | 93 | TempSumV+=VFilter[i]; |
emh203 | 0:085749c8446f | 94 | TempSumI+=IFilter[i]; |
emh203 | 0:085749c8446f | 95 | } |
emh203 | 0:085749c8446f | 96 | |
emh203 | 0:085749c8446f | 97 | ExtADCRobotCurrent = TempSumV>>4; |
emh203 | 0:085749c8446f | 98 | ExtADCRobotVoltage = TempSumI>>4; |
emh203 | 0:085749c8446f | 99 | |
emh203 | 0:085749c8446f | 100 | RobotBusVoltageHR = ((((float)(ExtADCRobotVoltage)/(32768.0f)) * HighResRobotBusVoltageScalingFactor + HighResRobotBusVoltageOffset))- VOFFSET_ADJUST; |
emh203 | 0:085749c8446f | 101 | RobotBusCurrentHR = (((((float)(ExtADCRobotCurrent)/(32768.0f)) * HighResRobotBusCurrentScalingFactor) -ACS576_VOFFSET) * ACS576_ISCALE) - ACS576_IOFFSET_TRIM; |
emh203 | 0:085749c8446f | 102 | |
emh203 | 0:085749c8446f | 103 | if(SystemState == SYSTEM_STATE_LOGGING) |
emh203 | 0:085749c8446f | 104 | { |
emh203 | 0:085749c8446f | 105 | |
emh203 | 0:085749c8446f | 106 | if (MyDataBlock.WriteOutPtr >MyDataBlock.ReadInPtr) |
emh203 | 0:085749c8446f | 107 | { |
emh203 | 0:085749c8446f | 108 | ReadWriteDifferential = (DATA_BLOCK_SIZE - MyDataBlock.WriteOutPtr + MyDataBlock.ReadInPtr); |
emh203 | 0:085749c8446f | 109 | } |
emh203 | 0:085749c8446f | 110 | else |
emh203 | 0:085749c8446f | 111 | { |
emh203 | 0:085749c8446f | 112 | ReadWriteDifferential = ( MyDataBlock.ReadInPtr - MyDataBlock.WriteOutPtr); |
emh203 | 0:085749c8446f | 113 | } |
emh203 | 0:085749c8446f | 114 | |
emh203 | 0:085749c8446f | 115 | if(ReadWriteDifferential >= DATA_BLOCK_SIZE - 1) |
emh203 | 0:085749c8446f | 116 | { |
emh203 | 0:085749c8446f | 117 | DataLogError = TRUE; |
emh203 | 0:085749c8446f | 118 | ErrorMsg = "Overrun!"; |
emh203 | 0:085749c8446f | 119 | } |
emh203 | 0:085749c8446f | 120 | else |
emh203 | 0:085749c8446f | 121 | { |
emh203 | 0:085749c8446f | 122 | MyDataBlock.Voltage[MyDataBlock.ReadInPtr] = RobotBusVoltageHR; |
emh203 | 0:085749c8446f | 123 | MyDataBlock.Current[MyDataBlock.ReadInPtr] = RobotBusCurrentHR; |
emh203 | 0:085749c8446f | 124 | |
emh203 | 0:085749c8446f | 125 | MyDataBlock.ReadInPtr++; |
emh203 | 0:085749c8446f | 126 | |
emh203 | 0:085749c8446f | 127 | if(MyDataBlock.ReadInPtr == DATA_BLOCK_SIZE) |
emh203 | 0:085749c8446f | 128 | { |
emh203 | 0:085749c8446f | 129 | MyDataBlock.ReadInPtr = 0; |
emh203 | 0:085749c8446f | 130 | } |
emh203 | 0:085749c8446f | 131 | } |
emh203 | 0:085749c8446f | 132 | } |
emh203 | 0:085749c8446f | 133 | else |
emh203 | 0:085749c8446f | 134 | { |
emh203 | 0:085749c8446f | 135 | ReadWriteDifferential = 0; |
emh203 | 0:085749c8446f | 136 | } |
emh203 | 0:085749c8446f | 137 | } |
emh203 | 0:085749c8446f | 138 | } |
emh203 | 0:085749c8446f | 139 | |
emh203 | 0:085749c8446f | 140 | |
emh203 | 0:085749c8446f | 141 |