Code: Select all
program joystick
symbol AbilDx = LATC.13
symbol AbilSx = LATC.14
Symbol SignDx = LATE.1
Symbol SignSx = LATE.4
'const half = 512
const deadband = 50
Const PX = 0
Const PY = 1
const Ref = 2
Const PT = 2
const PC = 3
const PWMdx = 1
const PWMsx = 2
const fatcor= 0.76 ' 0.63
const VaiX = 1
Const VaiY = 1
Const Stampa= 1
const ZeroLed = 2000
const AccTrig = 30
dim half as integer
dim lampled as word
dim AccDel as word
dim RealDutyX, RealDutyY as word
dim dutyX as word
dim dutyY as word
dim duty_50 as word
dim duty_100 as word
' hDuty
sub procedure Timer1Int org $1A
'** it is necessary to clear manually the interrupt flag as
IFS0 = IFS0 and $FFF7 ' clear TMR1IF
'** user code starts here
if lampled > ZeroLed then
LATB.5 = 1 xor LATB.5 ' invert PORTB
lampled = 0
else
inc(lampled)
end if
if 0 then
if AccDel > AccTrig then
if DutyX > RealDutyX then
inc(RealDutyX)
end if
if DutyX < RealDutyX then
Dec(RealDutyX)
end if
if DutyY > RealDutyY then
inc(RealDutyY)
end if
if DutyY < RealDutyY then
Dec(RealDutyY)
end if
else
inc(AccDel)
end if
end if
'** user code ends here
end sub
main:
dim potX as integer
dim potY as integer
dim tmp as integer
dim flta,fltb as float
dim ZeroX as integer
dim ZeroY as integer
dim Sat as word
' dim prova as integer[10][10]
dim nul as integer
dim i as byte
dim enable as byte
dim txt as string [10]
'set
ADPCFG = $FFF8
ADCON2 = ADCON2 and $0FFF
TRISB = 7
TRISC = 0
TRISD = 0
TRISE = 0
TRISF = 0
IPC0 = IPC0 or $1000 ' interrupt priority level = 1
IFS0 = IFS0 and $FFF7 ' clear TMR1IF
IEC0 = IEC0 or $0008 ' enable Timer1 interrupts
T1CON = $8030 ' Timer1 ON, internal clock FCY, prescaler 1 as 256
PR1 = 1
duty_50 = Pwm1_Mc_Init(5000, 1, $21, 0)
duty_100 = (duty_50 << 1) '* 0.9
Pwm1_Mc_Set_Duty(0, 1)
Pwm1_Mc_Set_Duty(0, 2)
Pwm1_Mc_Start()
RealDutyX = 0
RealDutyY = 0
' While 1
' latb.5 = 1
' nop
' latb.5=0
' nop
' wend
half=0
potx=half
poty=half
enable = 0
i=0
'Uart1_Init_Advanced(9600, 0, 1)
Uart1_Init(9600)
SignDx = 0
SignSx = 0
while i < 100
Tmp = PotX+Adc1_Read(PX)
potX = Tmp >> 1
Tmp = PotY+Adc1_Read(PY)
potY = Tmp >> 1
inc(i)
half = half+Adc1_Read(Ref)
half = half >> 1
wend
i=0
While 1
ZeroX = 0
ZeroY = 0
while i < 10
potX = Adc1_Read(PX)
potY = Adc1_Read(PY)
half =Adc1_Read(Ref)
' half = 512
' tmp = half+Adc_Read(Ref)
' half = tmp >> 1
' flta = flta - half
' fltb = fltb - half
flta = potx
fltb = poty
nul = half
tmp=(potx-half)
nop
nop
flta = tmp
tmp=(poty-half)
fltb=tmp
nop
nop
' fltb=(fltb-nul)
' potx=flta '*fatcor)
'poty=fltb '*fatcor)
'potX = potx+(flta*fatcor)
'potY = potY+(fltb*fatcor)
potX = (flta*fatcor)
potY = (fltb*fatcor)
ZeroX = ZeroX + PotX
ZeroY = ZeroY + PotY
ZeroX = ZeroX >>1
ZeroY = ZeroY >>1
' potX = potx>>1
' PotY = potY>>1
inc(i)
wend
i=0
PotX=ZeroX
PotY=ZeroY
'flta = Adc_Read(PX)
'fltb = Adc_Read(PY)
'half = Adc_Read(Ref)
'flta = flta - half
'fltb = fltb - half
'potX = (flta*fatcor)
'potY = (fltb*fatcor)
' nul = (potX-duty_50)<<2
if VaiX then
'nul = potX <<2
nul = potX
if fabs(nul) > duty_50 then
if nul < 0 then
tmp=-1
else
tmp=1
end if
nul=duty_50*tmp
end if
if fabs(nul) > deadband then
enable = 1
DutyX = nul+duty_50
DutyY = Nul+duty_50
else
enable = 0
'AbilDx = 0
'AbilSx = 0
DutyX = duty_50
DutyY = duty_50
end if
else
DutyX = duty_50
DutyY = duty_50
end if
' DutyX = duty_50
' DutyY = duty_50
' nul = (potY - duty_50)<<2
'' nul = potY <<1
' nul = nul/2
nul = potY
'nul = nul/2
if fabs(nul) > duty_50 then
if nul < 0 then
tmp=-1
else
tmp=1
end if
nul=duty_50*tmp
end if
if VaiY then
if fabs(NUL) > deadband then
enable = 1
tmp = DutyX+nul
if tmp < 0 then
tmp = 0
end if
if tmp > duty_100 then
tmp = duty_100
end if
DutyX = tmp
tmp = DutyY - nul
if tmp < 0 then
tmp = 0
end if
if tmp > duty_100 then
tmp = duty_100
end if
DutyY = tmp
end if
end if
AbilDx = enable
AbilSx = enable
if 0 then
nul = DutyX - Duty_50
If nul <= 0 then
DutyX= nul *-2
SignDx = 1
else
DutyX = nul * 2
SignDx = 0
End if
nul = DutyY - Duty_50
If nul <= 0 then
DutyY= nul *-2
SignSx = 1
else
DutyY = nul * 2
SignSx = 0
End if
end if
' Pwm_Mc_Set_Duty(DutyX, 1)
' Pwm_Mc_Set_Duty(DutyY, 2)
' dutyx=potX
' dutyY=poty
Pwm1_Mc_Set_Duty(DutyX, 1)
Pwm1_Mc_Set_Duty(DutyY, 2)
' Pwm_Mc_Set_Duty(RealDutyX, 1)
' Pwm_Mc_Set_Duty(RealDutyY, 2)
if Stampa then
wordToStr(DutyX,txt)
'intToStr(Poty,txt)
Uart1_Write_Text(txt)
Uart1_Write(32)
Uart1_Write(32)
' intToStr(Potx,txt)
wordToStr(DutyY,txt)
' wordToStr(potx,txt)
Uart1_Write_Text(txt)
Uart1_Write(13)
Uart1_Write(10)
end if
wend
end.
0 1 mBDsPic.exe -DBG -pP30F2010 -MSF -Y -DL -O11111114 -fo7.37 -N"C:\Luca\PICode\Kalman-PRO\joystic2.mbpds" -SP"C:\Program Files\Mikroelektronika\mikroBasic PRO for dsPIC\defs\" -SP"C:\Program Files\Mikroelektronika\mikroBasic PRO for dsPIC\uses\" -SP"C:\Luca\PICode\Kalman-PRO\" "__Lib_Math.mcl" "__Lib_MathDouble.mcl" "__Lib_System.mcl" "__Lib_Delays.mcl" "__Lib_String.mcl" "__Lib_Conversions.mcl" "__Lib_Trigon.mcl" "__Lib_ADC_1_p24FJ_p30.mcl" "__Lib_PWM_MC_1_p30_p33.mcl" "__Lib_UART_1_p30.mcl" "joystick2.mbas"
0 132 Compilation Started C:\Luca\PICode\Kalman-PRO\joystick2.mbas
1 1015 Hint: Compiling unit "C:\Luca\PICode\Kalman-PRO\joystick2.mbas" joystick2.mbas
42 1022 Hint: interrupt handler (Timer1Int) joystick2.mbas
230 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
231 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
239 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
243 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
261 1009 Warning: Implicit typecast performed from "real" to "integral" joystick2.mbas
262 1009 Warning: Implicit typecast performed from "real" to "integral" joystick2.mbas
311 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
311 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
322 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
322 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
352 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
352 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
366 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
366 1009 Warning: Implicit typecast performed from "integral" to "real" joystick2.mbas
479 1010 Hint: Unit "joystick2.mbas" has been recompiled joystick2.mbas
0 133 Compiled Successfully C:\Luca\PICode\Kalman-PRO\joystick2.mbas
0 138 All files Compiled in 94 ms
0 304 bad argument allignment bad argument allignment
0 452 Unsuccessful linking 'joystic2.mbpds' Unsuccessful linking 'joystic2.mbpds'
0 102 Finished (with errors): 14 Dec 2009, 22:02:07 joystic2.mbpds
Thanks
