Atan_CORDIC and GCBasic

[alejandro1957]

as the title.I'm still losing the brains behind this code to handle a form HMC5883L.
The module works great and interacts with the pic via I2C.
I use the code in a program in BASIC language (Gambas2) and works of wonder.
brought on pic 16f628, but goes with random numbers.
place the code in the pic and the tutorial where I got inspiration.

CHIP 16F628,4
CONFIG INTRC_OSC_NOCLKOUT,MCLR_OFF,WDT_OFF,PWRTE_ON,CP_OFF,LVP_OFF,DATA_CP_OFF

DIM ATAN_Table(15),Angulo AS INTEGER
DIM Puntador AS INTEGER

ATAN_Table(0)=11520
ATAN_Table(1)=6801
ATAN_Table(2)=3593
ATAN_Table(3)=1824
ATAN_Table(4)=916
ATAN_Table(5)=458
ATAN_Table(6)=229
ATAN_Table(7)=115
ATAN_Table(8)=57
ATAN_Table(9)=28
ATAN_Table(10)=14
ATAN_Table(11)=7
ATAN_Table(12)=4
ATAN_Table(13)=2
ATAN_Table(14)=1

FUNCTION Shr(Numero AS INTEGER,Contador AS INTEGER) 'AS INTEGER
DIM Index AS INTEGER
FOR Index=1 TO Contador
ROTATE Numero RIGHT
NEXT
Shr=Numero
END FUNCTION

FUNCTION Shl(Numero AS INTEGER,Contador AS INTEGER) 'AS INTEGER
DIM Index AS INTEGER
FOR Index=1 TO Contador
ROTATE Numero LEFT
NEXT
Shl=Numero
END FUNCTION

FUNCTION CordicAtan2(Y_Cordic AS INTEGER,X_Cordic AS INTEGER) AS INTEGER
DIM Resultado AS INTEGER
IF X_Cordic=0&Y_Cordic=0 THEN Resultado=0
IF X_Cordic>0&Y_Cordic>0 THEN Resultado=CordicAtan(Y_Cordic,X_Cordic)/256 'I°
IF X_Cordic<0&Y_Cordic>0 THEN Resultado=(90-(CordicAtan(Y_Cordic,Abs(X_Cordic))/256))+90 'II°
IF X_Cordic<0&Y_Cordic<=0 THEN Resultado=(CordicAtan(Abs(Y_Cordic),Abs(X_Cordic))/256)+180 'III°
IF X_Cordic>0&Y_Cordic<0 THEN Resultado=360-(CordicAtan(Abs(Y_Cordic),X_Cordic)/256) 'IV°
CordicAtan2=Resultado
END FUNCTION

FUNCTION CordicAtan(Axis_Y AS INTEGER,Axis_X AS INTEGER) AS INTEGER
DIM Xnew,Ynew,Angulo,Index,Y_tmp,X_tmp,Tmp,Contador AS INTEGER
Y_tmp=Axis_Y
X_tmp=Axis_X
Angulo=0
FOR Contador=0 TO 14
IF Y_tmp<0 THEN
Xnew=X_tmp-(Shr(Y_tmp,Contador))
Ynew=Y_tmp+(Shr(X_tmp,Contador))
Angulo-=ATAN_Table(Contador)
'END IF
ELSE
'IF Y_tmp>0 THEN
Xnew=X_tmp+(Shr(Y_tmp,Contador))
Ynew=Y_tmp-(Shr(X_tmp,Contador))
Angulo+=ATAN_Table(Contador)
END IF
X_tmp=Xnew
Y_tmp=Ynew
NEXT
CordicAtan=Angulo
END FUNCTION

SUB Write_Eeprom(In Address,In Dato)
EEADR=Address
EEDATA=Dato
INTOFF
SET EECON1.WREN ON
EECON2=0x55
EECON2=0xAA
SET EECON1.WR ON
SET EECON1.WREN OFF
WAIT UNTIL PIR1.EEIF=1
PIR1.EEIF=0
INTON
END SUB

Inicio:
TRISA=b'00000000'
TRISB=b'00000000'
PORTA=b'00000000'
PORTB=b'00000000'
PCON=b'00001000'
INTCON=b'00000000'
CVRCON=b'00000000'
CMCON=b'00000111'
CCP1CON=b'00000000'
WAIT 1 s
Write_Eeprom(0x00,CordicAtan2(1,252)) '0°
WAIT 25 ms
Write_Eeprom(0x01,CordicAtan2(177,1)) '89°
WAIT 25 ms
Write_Eeprom(0x02,CordicAtan2(1,-208)) '180°
WAIT 25 ms
Write_Eeprom(0x03,CordicAtan2(-171,-202)) '220°
WAIT 25 ms

and I read this on the eeprom.

alejandro@Alejandro:~$ pk2cmd -PPIC16F628 -GE 0-0Xff
Read successfully.

EEData Memory
0000 00 02 B4 B6 FF FF FF FF
0008 FF FF FF FF FF FF FF FF
0010 FF FF FF FF FF FF FF FF
..
..
..

0x00 = 0 value sought 1
0x02 = 2 value sought 89
0xB4 = 180 value sought 180
0xB6 = 182 value sought 220

I do not know where there is the error where I'm wrong.

like this

[kent_twt4]

Because you are trying to store an integer in eeprom (gcbasic doesn't do that per help)? Best of luck with this approach. I looked at the cordic function a long time ago when gcbasic wasn't as functional as it is today. Since my app was just looking at an inclinometer, then a table sufficed for just a single quadrant.

[alejandro1957]

Because you are trying to store an integer in eeprom
was to test the code on the fly on a breadboard
Now I'm connecting the pic to a PC via RS232
I looked at the cordic function a long time ago when gcbasic wasn't as functional as it is today. Since my app was just looking at an inclinometer, then a table sufficed for just a single quadrant.
ok, could you kindly send me the code for comparison with and see where my mistake?
Thanks.

[kent_twt4]

Hopefully the RS232 debug gets you where you need to be.

I said "looked at" not solved arctan function, haha. Basically I was trying to port the Microchip math library function. That was in assembly, and tried to port to gcb, but gave up on that.

My mission at the time was to get readings from a Dimension Engr. DE-ACCM2G sensor which is a 2 dimensional accelerometer. The accelerometer in that sensor is the ADXL322.

The code below really has no bearing on your magnetometer, but maybe the methods used might inspire someone? Here I have:
1) Committed a Sin-1 table to word values
2) Limited the table values up to 25 degrees as a test
3) The confusing table code was implemented before or around the time that table values were limited to byte values.
4) Initiated those values into an array on startup
5) Read the AD value in and use a brute force method to sort thru the table/array values till it is exceeded. Then you know the Degree value (i.e. table location).
6) I thought about using the eeprom, but discarded that approach.
7) Sorry for the extra debug and general mess the code is in.

'DimensionEngr DE-ACCM2G with Analog Devices ADXL322
'Chip model 
#chip 16f877a,20
'#config MCLRE = On 

#define LCD_IO 4
#define LCD_DB4 PORTD.4
#define LCD_DB5 PORTD.5
#define LCD_DB6 PORTD.6
#define LCD_DB7 PORTD.7
#define LCD_RS PORTC.3
#define LCD_RW PORTD.0
#define LCD_Enable PORTD.1
#define  Transmit PORTA.0
dir PortA.0 out
dir PortA.5 in  ;AN4

dim YaxisG as word
dim SINY as word
dim Degree as word
'dim Angle as word
dim SINarray(52)

'For AngleY = 1 to 180 Step 2

'ReadTable SINinverse, AngleY, SINY_H
'AngleY += 1
'ReadTable SINinverse, AngleY, SINY
'Angle(AngleY)= SINY

'Angle(AngleY)= 256*SINY_H + SINY
'Next
'**********EEPROM CODE************
'dim Angle(6)
'For EPcount = 0 to 5
EPWrite (0, 0)
EPWrite (1, 174)
EPWrite (2, 1)
EPWrite (3, 93)
EPWrite (4, 0x02)
EPWrite (5, 0x0B)

    'EEPROM address, 
    'Angle(AngleY)= EEPROM address, 

'**********************
cls

Print "Dimension Engr 2G Ac"

;Put SIN table values into SINarray for fast access
ReadSINTable

Main:

'At 5V g = +/- 0.750V, and at 10bit ADC, 5V/1023bits = .00489V/bit
'Then (.004888V/bit)/(0.750V/g) = 0.006517g/bits
'For example ADC value is 560 bits
'subtract the zero value (560-512=48 bits)
'Sin-1(angle)= 48 bits * 0.006517 g/bits = 0.3128g OR the angle is 18.23 degrees

YaxisG = ReadAD10(AN4)
wait 10 ms
YaxisG = (YaxisG-512)*65
Locate 1,0
If YaxisG >= 10000 then PRINT "Calc ":Print YaxisG
If YaxisG >= 1000 AND YaxisG < 10000 then Print "Calc  ":PRINT YaxisG
If YaxisG >= 100 AND YaxisG < 1000 then PRINT "Calc   ":PRINT YaxisG
If YaxisG >= 10 AND YaxisG < 100 then Print "Calc    ":PRINT YaxisG
If YaxisG < 10 AND YaxisG < 10 then Print "Calc     ":PRINT YaxisG

SinY = 0
'For SinAngle = 2 to 52 step 2
SinAngle = 2
Do Until SINY >= YaxisG
    'locate 2,1
    'Print SinAngle
'SINY_H = SINarray(SinAngle)
'nop
    'LCDHex SINarray(SinAngle):Print " "
'SinAngle += 1
'   SINY = SINarray(SinAngle)
'nop
    'LCDHex SINarray(SinAngle)
SINY = 256*SINarray(SinAngle) + SINarray(SinAngle+1)
SinAngle += 2
nop
locate 2,0
'Print SINY
If SINY >= 10000 then PRINT "Table":Print SINY
If SINY >= 1000 AND SINY < 10000 then Print "Table ":PRINT SINY
If SINY >= 100 AND SINY < 1000 then PRINT "Table  ":PRINT SINY
If SINY >= 10 AND SINY < 100 then Print "Table   ":PRINT SINY
If SINY < 10 AND SINY < 10 then Print "Table    ":PRINT SINY
Loop

locate 3,0
If (SinAngle-1) / 2 >= 10 Then
    Print "Angle ": Print ((SinAngle - 1) / 2)
Else
    Print "Angle  ":Print ((SinAngle- 1) / 2)
End if

wait 1 s
Goto Main 
;------------------------------------------------

Sub ReadSINTable
'locate 2,0
'Print "SINtable"
For tableNum = 2 to 51  ;i.e. 2 to 52 because of incr. 
    'locate 3,0
ReadTable SINinverse, tableNum, SINY_H
SINarray(tableNum) = SINY_H
tableNum += 1
ReadTable SINinverse, tableNum, SINY
    'SINY = 256*SIN_H + SINY
SINarray(tableNum) = SINY
    'If YaxisG < SINY Then
    '   Print tableNum
    '   goto exitTable
    'End if

'If SINY >= 10000 then PRINT SINY
'If SINY >= 1000 AND SINY < 10000 then Print " ":PRINT SINY
'If SINY >= 100 AND SINY < 1000 then PRINT "  ":PRINT SINY
'If SINY >= 10 AND SINY < 100 then Print "   ":PRINT SINY
'If SINY < 10 AND SINY < 10 then Print "    ":PRINT SINY
'Wait 3 sec
Next
end sub

'For Wordvalue = 1 to 3 ;number of words read
Sub EPSinTable
For NumDeg = 1 to 5 step 2 ;number of words read 
'locate 1,0
'Print " "  
NumDeg -= 1  ;For/Next doesn't work with 0 to X
'EPRead (NumDeg, DegreeH)
NumDeg += 1
'EPRead (NumDeg, DegreeL)
Degree = DegreeH * 256 + DegreeL
    'Print (Degree)
    'Print "  "
    '0,174,01,93,01,211
wait 1 s
next
end sub

Table SINinverse  ;GCBasic likes decimal no.'s
;GCBasic inserts length of table in first bytes location
0X00  ;bump table to even no. because 0(even) occcupied by size
0x00  ;begin table values at addr. 2 (even no.)
0xAE  ;dec 174  1 degree
0x01
0x5D  ;dec 349
0x02
0x0B  ;dec 523
0x02
0xBA  ;dec 698
0x03
0x68  ;dec 872  5 degrees
0x04
0x15  ;dec 1045
0x04
0xC3  ;dec 1219
0x05
0x70  ;dec 1392
0x06
0x1C  ;dec 1564
0x06
0xC8  ;dec 1736  10 degrees
0x07
0x74  ;dec 1908
0x08
0x1F  ;dec 2079
0x08
0xC9  ;dec 2249
0x09
0x73  ;dec 2419
0x0A
0x1C  ;dec 2588  15 degrees
0x0A
0xC4  ;dec 2756
0x0B
0x6C  ;dec 2924
0x0C
0x12  ;dec 3090
0x0C
0xB8  ;dec 3256
0x0D
0x5C  ;dec 3420  20 degrees
0x0E
0x00  ;dec 3584
0x0E
0xA2  ;dec 3746
0x0F
0x43  ;dec 3907
0x0F
0xE3  ;dec 4067
0x10
0x82  ;dec 4226  25 degrees

'4384
'4540
'4695
'4848
'5000  30 degrees
'5150
'5299
'5446
'5592
end table

[alejandro1957]

thanks.
I did the math by hand CORDIC algorithm for the function ATAN.
the result is the same as the program atan2 in Gambas2.
see txt file

[alejandro1957]

then this code works fine with 16F876A and module HMC5883L(Chinese)

CHIP 16F876A,4
CONFIG OSC=XT,WDT_OFF,PWRTE_ON,CP_OFF,DEBUG_OFF,WRT_OFF,CPD_OFF,LVP_OFF,BODEN_ON

DIM Flag_Stop,Puntador,Indice,Valor,Repite AS INTEGER
DIM Axis_Y,Axis_X,Axis_Z AS WORD

SUB HMC5883L_Init()
START()
Tx_I2C(0x3C)
Tx_I2C(0x00)
Tx_I2C(0x70)
STOP()
WAIT 5 ms
START()
Tx_I2C(0x3C)
Tx_I2C(0x01)
Tx_I2C(0xA0)
STOP()
WAIT 5 ms
END SUB

FUNCTION HMC5883L_Test() AS BYTE
DIM Test AS BYTE
START()
Tx_I2C(0x3C)
Tx_I2C(0x02)
RESTART()
Tx_I2C(0x3D)
Test=Rx_I2C()
STOP()
HMC5883L_Test=Test
END FUNCTION

SUB HMC5883L_Lectura()
DIM X_Tmp(2),Z_Tmp(2),Y_Tmp(2) AS INTEGER
START()
Tx_I2C(0x3C)
Tx_I2C(0x03)
RESTART()
Tx_I2C(0x3D)
X_Tmp(0)=Rx_I2C()
ACK_I2C()
X_Tmp(1)=Rx_I2C()
ACK_I2C()
Z_Tmp(0)=Rx_I2C()
ACK_I2C()
Z_Tmp(1)=Rx_I2C()
ACK_I2C()
Y_Tmp(0)=Rx_I2C()
ACK_I2C()
Y_Tmp(1)=Rx_I2C()
No_ACK_I2C()
STOP()
Axis_X=X_Tmp(0) * 256
Axis_X+=X_Tmp(1)
Axis_X=Signo(Axis_X)
Axis_Y=Y_Tmp(0) * 256
Axis_Y+=Y_Tmp(1)
Axis_Y=Signo(Axis_Y)
Axis_Z=Z_Tmp(0) * 256
Axis_Z+=Z_Tmp(1)
Axis_Z=Signo(Axis_Z)
END SUB

FUNCTION Signo(Valor AS WORD) AS WORD
IF Valor<2047 THEN
Signo=Valor
ELSE
Signo=Valor-65535
END IF
END FUNCTION

SUB HMC5883L_Mod_Single()
START()
Tx_I2C(0x3C)
Tx_I2C(0x02)
Tx_I2C(0x00)
STOP()
WAIT 10 ms
END SUB

SUB HMC5883L_Mod_Continuo()
START()
Tx_I2C(0x3C)
Tx_I2C(0x02)
Tx_I2C(0x01)
STOP()
WAIT 10 ms
END SUB

SUB I2C_Init(FClock AS INTEGER,BitRate AS INTEGER)
SSPSTAT=b'11000000'
SSPADD=((FClock/BitRate)/4)-1
SSPCON2=0
SSPCON=b'00111000'
SET PIR1.SSPIF OFF
SET PIR2.BCLIF OFF
END SUB

SUB W_MSSP()
DO
LOOP UNTIL PIR1.SSPIF=1
PIR1.SSPIF=0
END SUB

SUB START()
SET SSPCON2.SEN ON
W_MSSP()
END SUB

SUB RESTART()
SET SSPCON2.RSEN ON
W_MSSP()
END SUB

SUB STOP()
SET SSPCON2.PEN ON
W_MSSP()
END SUB

SUB ACK_I2C()
SET SSPCON2.ACKDT OFF
SET SSPCON2.ACKEN ON
W_MSSP()
END SUB

SUB No_ACK_I2C()
SET SSPCON2.ACKDT ON
SET SSPCON2.ACKEN ON
W_MSSP()
END SUB

FUNCTION CheckACK_I2C()
IF SSPCON2.ACKSTAT=0 THEN
CheckACK_I2C=0
ELSE
CheckACK_I2C=1
END IF
END FUNCTION}

SUB Wait_I2C()
WAIT WHILE (SSPCON2&0x1F)|(SSPSTAT&0x04)
END SUB

SUB Tx_I2C(SendByte)
SSPBUF=SendByte
W_MSSP()
DO
LOOP UNTIL SSPCON2.ACKSTAT=0
END SUB

FUNCTION Rx_I2C() AS BYTE
SET SSPCON2.RCEN ON
WAIT UNTIL SSPSTAT.BF=1
Rx_I2C=SSPBUF
W_MSSP()
END FUNCTION

TABLE ATAN_Table
11520
6801
3593
1824
916
458
229
115
57
28
14
7
4
2
1
END TABLE

FUNCTION SHIFT_Dx(Valor AS INTEGER,Rotacion AS INTEGER) AS INTEGER
DIM Resultado,Contador AS INTEGER
FOR Contador=0 TO Rotacion-1
Valor=Valor/2
NEXT
SHIFT_Dx=Valor
END FUNCTION

FUNCTION CordicAtan(AxisY AS INTEGER,AxisX AS INTEGER) AS INTEGER
DIM Xnew,Ynew,Angulo,Index,EjeY_tmp,EjeX_tmp,Table_tmp AS INTEGER
EjeY_tmp=AxisY
EjeX_tmp=AxisX
Angulo=0
FOR Index=0 TO 14
READTABLE ATAN_Table,Index+1,Table_tmp
IF EjeY_tmp<0 THEN
Xnew=EjeX_tmp-SHIFT_Dx(EjeY_tmp,Index)
Ynew=EjeY_tmp+SHIFT_Dx(EjeX_tmp,Index)
Angulo-=Table_tmp
ELSE
Xnew=EjeX_tmp+SHIFT_Dx(EjeY_tmp,Index)
Ynew=EjeY_tmp-SHIFT_Dx(EjeX_tmp,Index)
Angulo+=Table_tmp
END IF
EjeX_tmp=Xnew
EjeY_tmp=Ynew
NEXT
CordicAtan=Angulo
END FUNCTION

FUNCTION ATAN2(AxisY AS INTEGER,AxisX AS INTEGER) AS INTEGER
DIM Resultado AS INTEGER
IF AxisX=0&AxisY=0 THEN Resultado=0
IF AxisX>0&AxisY>0 THEN Resultado=CordicAtan(AxisY,AxisX)/256
IF AxisX<0&AxisY>0 THEN Resultado=180-(CordicAtan(AxisY,Abs(AxisX))/256)
IF AxisX<0&AxisY<=0 THEN Resultado=(CordicAtan(Abs(AxisY),Abs(AxisX))/256)+180
IF AxisX>=0&AxisY<=0 THEN Resultado=359-(CordicAtan(Abs(AxisY),AxisX)/256)
ATAN2=Resultado
END FUNCTION

Ciclo:
TRISA=b'000000'
TRISB=b'00000000'
TRISC=b'00011000'
PORTA=b'000000'
PORTB=b'00000000'
PORTC=b'00000000'
INTCON=b'00000000'
CVRCON=b'00000000'
CCP1CON=b'00000000'
CCP2CON=b'00000000'
ADCON1=b'00000110'
I2C_Init(4000,100)
HMC5883L_Init()
WAIT 20 ms
IF HMC5883L_Test()=3 THEN
FOR Repite=1 TO 5
PORTC.5=1
WAIT 100 ms
PORTC.5=0
WAIT 100 ms
NEXT
END IF
WAIT 2 s
DO
HMC5883L_Mod_Single()
HMC5883L_Lectura()
WAIT 20 ms
Valor=ATAN2(Axis_Y,Axis_X)
IF (Valor>0&Valor<10)|(Valor>90&Valor<100)|(Valor>180&Valor<190)|(Valor>270&Valor<280)|(Valor>350&Valor<360) THEN
PORTC.5=1
WAIT 1 s
PORTC.5=0
END IF
LOOP