CPU 120
' TassyJim
' 16 October 2015
' MPU-6050 gyro/accelerometer
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Option AUTORUN ON
Option EXPLICIT
On ERROR IGNORE
Option baudrate 57600

Dim MPU = &h68 ' MPU-6050 address
Dim temp, accelX, accelY, accelZ, gyroX, gyroY, gyroZ, tilt, accelM
Dim gyro(14)
Dim R,RX,RY,RZ As Float
Dim ID,I2Cerr As Integer = 0

Const BME280_ADDRESS = &H76
Const BME280_REGISTER_T1 = &H88
Const BME280_REGISTER_P1 = &H8E
Const BME280_REGISTER_H1 = &HA1
Const BME280_REGISTER_H2 = &HE1
Const BME280_REGISTER_CHIPID = &HD0
Const BME280_REGISTER_CONTROLHUMID = &HF2
Const BME280_REGISTER_CONTROL = &HF4
Const BME280_REGISTER_PRESSUREDATA = &HF7
Const BME280_REGISTER_TEMPDATA = &HFA
Const BME280_REGISTER_HUMIDDATA = &HFD
'
Dim integer s16=&HFFFFFFFFFFFF0000 , s16b=&H8000
Dim integer s12=&HFFFFFFFFFFFFF000 , s12b=&H800
Dim integer s8= &HFFFFFFFFFFFFFF00 , s8b=&H80
'
Dim INTEGER T1,T2,T3 'uint16_t, int16_t, int16_t
Dim INTEGER P1,P2,P3,P4,P5,P6,P7,P8,P9 'uint16_t, 8 x int16_t
Dim INTEGER H1,H2,H3,H4,H5,H6 'uint8_t, int16_t , uint8_t, int16_t, int16_t, int8_t
'
Dim INTEGER t_fine 'used to store accurate temp reading from temp conversion for use in pressure and humidity conversio
Dim press,lasttemp,lasthum,lastalt As Float

SetPin 1,dout
SetPin 2,dout
SetPin 3,dout : Pin(3)=1
SetPin 21,ain
SetPin 22,ain
SetPin 27,Ain
SetPin 18,ain
SetPin 55,dout : Pin(55)=0

Print Chr$(27)+"[2J" ' VT100 CLS

I2Cinit
Do
Print Chr$(27)+"[1;1H"
DS3231
MPU6050
BME280
MCP9700
LEDs
Loop

Sub I2Cinit
I2C open 400,250 '400KHz bus speed
Pause 50
bme280_init
mpu6050_init
End Sub

Sub LEDs
Pin(1)=Not Pin(1)
End Sub

Sub DS3231
Print "DS3231 RTC.CLOCK--------------------------------"
RTC GETTIME
Print Time$,Date$,ID
ID=ID+1
End Sub

Sub MCP9700
Print "MCP 9700 TEMP.SENS + VIN------------------------"
Print "TEMP(9700) ",Str$(((Pin(18)-0.5)/0.01),3,1)
Print "V21="Str$(Pin(21)*10,2,2)
Print "V22="Str$(Pin(22)*10,2,2)
Print "VIN="Str$(Pin(27),2,2)
Print "MM.I2C="MM.I2C " n.err="I2Cerr
If MM.I2C>0 Then
Pin(2)=1
Pin(3)=0
I2Cerr=I2Cerr+1
I2C CLOSE
Pause 250
I2Cinit
Else
Pin(2)=0
End If
End Sub

Sub BME280
Print "BME280 (TEMP+PRESS+HUMID)SENS------------------"
If ID Mod 10 = 0 Then
lasttemp=bme280_read_temp()
Press=bme280_read_pressure()
lasthum=bme280_read_humidity()
lastalt=44330.0 * (1.0 - (press / 1013.25) ^ 0.1903))
End If
Print "TEMP(280) ",Str$(lasttemp,2,2)
Print "PRESS.ATM. ",Str$(press,4,1)
Print "HUMID ",Str$(lasthum,4,1)
Print "ALTITUDINE ",Str$(lastalt,4,2)
End Sub

Sub mpu6050_init
I2C WRITE MPU,0,2,&h6B,0 'wake up the MPU-6050
End Sub


Sub MPU6050
I2C WRITE MPU,0,2,&h6B,0 'wake up the MPU-6050
I2C WRITE MPU,1,1,&h3B 'Reset memory pointer.
I2C READ MPU,0,14,gyro(1) 'Read 14 bytes
accelX=sint16(gyro(1)*256+gyro(2))
accelY=sint16(gyro(3)*256+gyro(4))
accelZ=sint16(gyro(5)*256+gyro(6))
temp=sint16(gyro(7)*256+gyro(8))/340+36.53
gyroX=sint16(gyro(9)*256+gyro(10))
gyroY=sint16(gyro(11)*256+gyro(12))
gyroZ=sint16(gyro(13)*256+gyro(14))

accelM=Sqr(accelX*accelX+accelZ*accelZ)

If accelM=0 Then ' avoid divde by zero error
If accelY<0 Then
tilt=-90
Else
tilt=90
EndIf
ElseIf accelZ<0 Then
If accelY<0 Then
tilt= -180 - Deg(Atn(accelY/accelM))
Else
tilt= 180 - Deg(Atn(accelY/accelM))
EndIf
Else
tilt = Deg(Atn(accelY/accelM))
EndIf
Print "MPU6050 (ACC+GYRO+TEMP).SENS--------------------"
Print "ACCEL_XOUT ",Str$(accelX,6,0)
Print "ACCEL_YOUT ",Str$(accelY,6,0)
Print "ACCEL_ZOUT ",Str$(accelZ,6,0)
Print "TEMP(6050) ",Str$(temp,3,1)
Print "GYRO_XOUT ",Str$(gyroX,6,0)
Print "GYRO_YOUT ",Str$(gyroY,6,0)
Print "GYRO_ZOUT ",Str$(gyroZ,6,0)
Print "Degrees ",Str$(tilt,4,1)," "
R=Sqr(accelX*accelX+accelY*accelY+accelZ*accelZ)
If R<>0 Then
RX=-(ACos(accelX/R)*180/Pi)+90
RY=-(ACos(accelY/R)*180/Pi)+90
RZ=-(ACos(accelZ/R)*180/Pi)+90
Print "X,Y,Z inc. ",Str$(RX,4,1),Str$(RY,4,1),Str$(RZ,4,1)
End If
End Sub

Function sint16(x) ' convert to signed 16 bit number
If x>32767 Then
sint16 = x - 65536
Else
sint16 = x
EndIf
End Function

Function bme280_read_temp() As float
Local integer var1,var2,adc_T
Local adc%(2)
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_TEMPDATA
I2C read BME280_ADDRESS,0,3,adc%()
adc_T=((adc%(0)<<16) Or (adc%(1)<<8) Or adc%(2))>>4
var1 = ((((adc_T>>3) - (T1 <<1))) * T2) \ q(11)
var2 = (((((adc_T>>4) - (T1)) * ((adc_T\ q(4)) - (T1))) \ q(12)) * (T3)) \ q(14)
t_fine = var1 + var2
bme280_read_temp = ((t_fine * 5 + 128) \ q(8))/100.0
End Function

Function bme280_read_pressure() As float
Local integer var1, var2, adc_P, p
Local adc%(2)
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_PRESSUREDATA
I2C read BME280_ADDRESS,0,3,adc%()
adc_P=((adc%(0)<<16) Or (adc%(1)<<8) Or adc%(2))>>4
var1 = t_fine - 128000
var2 = var1 * var1 * P6
var2 = var2 + ((var1 * P5)<<17)
var2 = var2 + (P4 << 35)
var1 = ((var1 * var1 * P3)\ q(8)) + ((var1 * P2)<<12)
var1 = ((1<<47)+var1)*P1\ q(33)
If var1 = 0 Then
bme280_read_pressure = 0' avoid exception caused by division by zero
Exit Function
EndIf
p = 1048576 - adc_P
p = (((p<<31) - var2)*3125) \ var1
var1 = (P9 * (p\ q(13)) * (p\ q(13))) \ q(25)
var2 = (P8 * p) \ q(19)
p = ((p + var1 + var2) \ q(8)) + (P7<<4)
bme280_read_pressure = p/25600.0
End Function
'
Function bme280_read_humidity() As float
Local integer v_x1,adc_H
Local adc%(1)
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_HUMIDDATA
I2C read BME280_ADDRESS,0,2,adc%()
adc_H=(adc%(0)<<8) Or adc%(1)
v_x1 = t_fine - 76800
v_x1=(((((adc_H<<14)-((H4)<<20)-(H5*v_x1))+16384)\ q(15))*(((((((v_x1*H6)\ q(10))*(((v_x1*H3)\ q(11))+32768))\ q(10))+2097152)*H2+8192)\ q(14)))
v_x1 = (v_x1 - (((((v_x1 \ q(15)) * (v_x1 \ q(15))) \ q(7)) * (H1)) \ q(4)))
If v_x1< 0 Then v_x1 = 0
If v_x1 > 419430400 Then v_x1= 419430400
bme280_read_humidity = (v_x1\ q(12)) / 1024.0
End Function

Sub bme280_init
Local i%,cal%(17)
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_CHIPID
I2C read BME280_ADDRESS,0,1,i%
'If i%<>&H60 Then Print "Error BME280 not found"
'
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_T1
I2C read BME280_ADDRESS,0,6,cal%()
T1=cal%(0) Or (cal%(1)<< 8)
T2=cal%(2) Or (cal%(3)<< 8): If T2 And s16b Then T2=T2 Or s16 'sign extend if required
T3=cal%(4) Or (cal%(5)<< 8): If T3 And s16b Then T3=T3 Or s16
'
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_P1
I2C read BME280_ADDRESS,0,18,cal%()
P1=cal%(0) Or (cal%(1)<<8)
P2=cal%(2) Or (cal%(3)<<8): If P2 And s16b Then P2=P2 Or s16 'sign extend if required
P3=cal%(4) Or (cal%(5)<<8): If P3 And s16b Then P3=P3 Or s16
P4=cal%(6) Or (cal%(7)<<8): If P4 And s16b Then P4=P4 Or s16
P5=cal%(8) Or (cal%(9)<<8): If P5 And s16b Then P5=P5 Or s16
P6=cal%(10) Or (cal%(11)<<8): If P6 And s16b Then P6=P6 Or s16
P7=cal%(12) Or (cal%(13)<<8): If P7 And s16b Then P7=P7 Or s16
P8=cal%(14) Or (cal%(15)<<8): If P8 And s16b Then P8=P8 Or s16
P9=cal%(16) Or (cal%(17)<<8): If P9 And s16b Then P9=P9 Or s16
'
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_H1
I2C read BME280_ADDRESS,0,1,H1
I2C write BME280_ADDRESS,1,1,BME280_REGISTER_H2
I2C read BME280_ADDRESS,0,7,cal%()
H2=cal%(0) Or (cal%(1)<< 8): If H2 And s16b Then H2=H2 Or s16 'sign extend if required
H3=cal%(2)
H6=cal%(6): If H6 And s8b Then H6=H6 Or s8 'sign extend if required
H4=(cal%(3)<<4) Or (cal%(4) And &H0F): If H4 And s12b Then H4=H4 Or s12 'sign extend if required
H5=(cal%(5)<<4) Or (cal%(4)>>4): If H5 And s12b Then H5=H5 Or s12
'
I2C write BME280_ADDRESS,0,2,BME280_REGISTER_CONTROLHUMID,&H05 '16x oversampling humidity
I2C write BME280_ADDRESS,0,2,BME280_REGISTER_CONTROL,&HB7 '16x oversampling pressure/temp, normal mode
'
End Sub
'
Function q(x As integer) As integer 'returns 2 raised to the power
q=(1<<x)
End CFunction