Discussion about the ZBasic language including the System Library. If you're not sure where to post your message, do it here. However, do not make test posts here; that's the purpose of the Sandbox.
'OPTIONS
Option Com1Speed 115200
'PINS
Const gyro_y_PN as BYTE = 35
Const gyro_y_TEMP_PN as BYTE = 34
Const accel_y_PN as BYTE = 32
'VARS
Public gyro_y_RAW as INTEGER 'raw gyro adc read
Public gyro_y_DS as SINGLE 'gyro degrees/s
Public gyro_y_ANGLE as SINGLE 'integrated gyro angle in degrees
Public gyro_y_BAIS as INTEGER 'gyro bias/null value
Public accel_y_RAW as INTEGER 'raw accel adc read
Public accel_y_angle as SINGLE 'accel angle in degrees
Public y_angle as SINGLE 'correct angle estimation
PUBLIC TaskStack1(1 to 200) as Byte
'**********************************
Sub Main()
Call Delay(0.50)
CallTask "Accelerometer", TaskStack1 'get acceleromter data
'Set initial gyro bias, avg of 17 reads
gyro_y_BAIS = AvgADC(gyro_y_PN, 17)
Do 'Runs at 320hz
'get gyro data
gyro_y_RAW = AvgADC(gyro_y_PN, 10) - gyro_y_BAIS
gyro_y_DS = (CSng(gyro_y_RAW)) / 4.0 * 5.0 'calc degrees/s
gyro_y_angle = gyro_y_angle + (gyro_y_DS * 0.005) 'accumulator, for testing only.
y_angle = y_angle + (gyro_y_DS * 0.005) 'accumulate to be corrected
'Push pull into correct positon with factor of gyro vs accel error
if abs(accel_y_angle) < 36.0 Then
if y_angle > accel_y_angle then
y_angle = y_angle - Abs((y_angle - accel_y_angle) * 0.02)
elseif y_angle < accel_y_angle then
y_angle = y_angle + Abs((y_angle - accel_y_angle) * 0.02)
end if
End if
Loop
End Sub
Public Sub Accelerometer() 'Runs at 25Hz
Do
'accel
accel_y_RAW = GetADC(accel_y_PN)
accel_y_angle = ((Csng(accel_y_RAW) / 1024.0 * 5.0) - 2.46) / 0.07 'convert to degrees
Call Sleep(0.040)
Loop
End Sub
PUBLIC FUNCTION avgADC(ByVal PN as BYTE, ByVal Readings as INTEGER) as INTEGER
DIM Sensortotal as INTEGER
DIM i as INTEGER
Sensortotal = 0
FOR i = 1 to Readings
SensorTotal = SensorTotal + getADC(PN)
NEXT i
avgADC = (SensorTotal \ Readings)
END FUNCTION
For some reason the ADC read for the accelerometer is only working very intermitently (spelling), and just giving a static incorrect reading most of the time, with occasional flashes of the actual value.
Im guessing this has something to do with the tasks fighting over the ADC? Ive tried putting some locktasks in around the reads but it seems to make no difference. Any Ideas? Do all ADC reads have to be in the same TASK?
Also if there is any advice out there on how i can track the bias of the gyro on the fly that would be great!
sturgessb wrote:Im guessing this has something to do with the tasks fighting over the ADC? Ive tried putting some locktasks in around the reads but it seems to make no difference.
The ADC is a "resource" that must be used serially. If it is busy when GetADC() is called, you don't get a valid reading. In native mode, if the task is locked when GetADC() is called, it is supposed to loop, waiting for the conversion to complete before continuing. If the task is not locked, the ADC conversion is begun and then another task is allowed to run.
The best strategy may be to use a semaphore to control access to the ADC. Something like the (untested) code below may work.
Function readADC(ByVal pin as Byte) as UnsignedInteger
Static sem as Boolean
' wait for the ADC to be available
Do While Not Semaphore(sem)
Call Sleep(0.01)
Loop
' perform the conversion, release the semapthore
readADC = CUInt(GetADC(pin))
sem = false
End Function
sturgessb wrote:that works but slows things down a bit
That would seem to indicate that the ADC is a bottleneck. The Semaphore() call itself doesn't take much time but the Sleep() call when the ADC is not available may be able to be shortened. Worst case, the Sleep() call adds 10mS to the ADC acquisition time. It may work better to change it to Sleep(0). If you take out the Sleep() call, it will probably make it worse since the task waiting for the semaphore will likely run for its full time slice (1.95mS) before allowing the task that is using the ADC to get its turn to run at which time it will release the semaphore.
Another possible strategy is to write your own ADC code and use the "ADC Conversion Complete" interrupt as a trigger to retrieve the conversion value.
sturgessb wrote:I thought that Fix might have a decimal place variable but it doesnt.
Are you considering this for display purposes or for computational purposes? If all you are concerned with is presentation, use Fmt(). On the other hand, if you want to round/truncate to a certain number of decimal places there is more computation involved. The formula below will nominally truncate to three decimal places but, due to representational error and other errors, you may still see more than 3 decimal places if you were to print the resulting value.
Basically Im having this weird issue where my program is working fine for a few seconds and then it suddenly shows values as &.&
Example of my output...
ANGLES: 0.33,0.00,-0.05 BIAS:492.0 HZ: 288,23 RATES: 0.0373 , -0.0971 -8.9619E-03
Then after a few seconds....
ANGLES: 0.19,&.&&,&.&& BIAS:&.& HZ: 331,27 RATES: &.&&&& , &.&&&& &.&&&&
when this happens the HZ rate of my loop suddenly jumps up about 50hz which indicates to me that it is stopping doing the calculations, or i guess it could be that its quicker just to output a load of &s than the actual data?
what could be causing this, i dont think I'm going anywhere near the min max limit of the float.
sturgessb wrote:[W]hat causes the output to be &.& ? And how that that compare to this *.* which i also sometimes get.
The ampersand represents infinity (similar shapes) while the asterisk indicates NaN (not a number). These are both documented on the page describing Fmt().
Both infinity and NaN can result from operations with invalid operands, e.g. square root of a negative, dividing by zero, tan 90*, etc.