;***************************************************************************************** ; Mega 128 Scope Controller Program ; (c) Martin Cibulski ;***************************************************************************************** ;***************************************************************************************** ; Stepper Motor Datasets .DSEG mot: mot_a_ssp1_raw: .byte 4 mot_a_ssp2_raw: .byte 4 mot_a_ssp3_raw: .byte 4 mot_a_ssp4_raw: .byte 4 mot_a: mot_a_ssp1: .byte 4 mot_a_ssp2: .byte 4 mot_a_ssp3: .byte 4 mot_a_ssp4: .byte 4 mot_a_ssp5: .byte 4 mot_a_hssp: .byte 4 ;handbox speed setpoint mot_a_hsp: .byte 4 ;handbox speed mot_a_hacc: .byte 4 ;handbox acceleration mot_a_gssp: .byte 4 ;tracking/goto speed setpoint mot_a_gsp: .byte 4 ;tracking/goto speed mot_a_gacc: .byte 4 ;acceleration mot_a_sp: .byte 4 ;speed mot_a_pos: .byte 6 ;position mot_a_inverted: .byte 1 mot_b: mot_b_ssp1: .byte 4 mot_b_ssp2: .byte 4 mot_b_ssp3: .byte 4 mot_b_ssp4: .byte 4 mot_b_ssp5: .byte 4 mot_b_hssp: .byte 4 ;handbox speed setpoint mot_b_hsp: .byte 4 ;handbox speed mot_b_hacc: .byte 4 ;acceleration mot_b_gssp: .byte 4 ;tracking/goto speed setpoint mot_b_gsp: .byte 4 ;tracking/goto speed mot_b_gacc: .byte 4 ;acceleration mot_b_sp: .byte 4 ;speed mot_b_pos: .byte 6 ;position mot_b_inverted: .byte 1 mot_hsp_notzero: .byte 1 ;***************************************************************************************** ;Variables in Datasets .EQU MOT_SSP1 = (mot_a_ssp1-mot_a) ;4 bytes .EQU MOT_SSP2 = (mot_a_ssp2-mot_a) ;4 bytes .EQU MOT_SSP3 = (mot_a_ssp3-mot_a) ;4 bytes .EQU MOT_SSP4 = (mot_a_ssp4-mot_a) ;4 bytes .EQU MOT_SSP5 = (mot_a_ssp5-mot_a) ;4 bytes .EQU MOT_HSSP = (mot_a_hssp-mot_a) ;4 bytes .EQU MOT_HSP = (mot_a_hsp-mot_a) ;4 bytes .EQU MOT_HACC = (mot_a_hacc-mot_a) ;4 bytes .EQU MOT_GSSP = (mot_a_gssp-mot_a) ;4 bytes .EQU MOT_GSP = (mot_a_gsp-mot_a) ;4 bytes .EQU MOT_GACC = (mot_a_gacc-mot_a) ;4 bytes .EQU MOT_SP = (mot_a_sp-mot_a) ;4 bytes .EQU MOT_POS = (mot_a_pos-mot_a) ;6 bytes .EQU MOT_INVERTED = (mot_a_inverted-mot_a) ;1 byte .CSEG mot_label_tab: .dw mot_hsp_notzero .db L_BRAM ,"MOTNOTZERO " .dw mot_a_pos .db L_LRAM ,"MOTAPOS " .dw mot_a_sp .db L_LRAM ,"MOTASP " .dw mot_a_hssp .db L_LRAM ,"MOTAHSSP " .dw mot_a_hsp .db L_LRAM ,"MOTAHSP " .dw mot_a_hacc .db L_LRAM ,"MOTAHACC " .dw mot_a_gssp .db L_LRAM ,"MOTAGSSP " .dw mot_a_gsp .db L_LRAM ,"MOTAGSP " .dw mot_a_gacc .db L_LRAM ,"MOTAGACC " .dw mot_a_ssp1_raw .db L_LRAM ,"MOTASSP1 " .dw mot_a_ssp2_raw .db L_LRAM ,"MOTASSP2 " .dw mot_a_ssp3_raw .db L_LRAM ,"MOTASSP3 " .dw mot_a_ssp4_raw .db L_LRAM ,"MOTASSP4 " .dw mot_a_ssp5 .db L_LRAM ,"MOTASSP5 " .dw mot_a_inverted .db L_BRAM ,"MOTAINVERT " .dw mot_b_pos .db L_LRAM ,"MOTBPOS " .dw mot_b_sp .db L_LRAM ,"MOTBSP " .dw mot_b_hssp .db L_LRAM ,"MOTBHSSP " .dw mot_b_hsp .db L_LRAM ,"MOTBHSP " .dw mot_b_hacc .db L_LRAM ,"MOTBHACC " .dw mot_b_gssp .db L_LRAM ,"MOTBGSSP " .dw mot_b_gsp .db L_LRAM ,"MOTBGSP " .dw mot_b_gacc .db L_LRAM ,"MOTBGACC " .dw mot_b_ssp1 .db L_LRAM ,"MOTBSSP1 " .dw mot_b_ssp2 .db L_LRAM ,"MOTBSSP2 " .dw mot_b_ssp3 .db L_LRAM ,"MOTBSSP3 " .dw mot_b_ssp4 .db L_LRAM ,"MOTBSSP4 " .dw mot_b_ssp5 .db L_LRAM ,"MOTBSSP5 " .dw mot_b_inverted .db L_BRAM ,"MOTBINVERT " .dw 0 .db L_END , 0 ;***************************************************************************************** ;Calculate new speed setpoints for azimut movements ;Then movements in AZ or ALT will have the same speed ON THE SKY even near the zenit. ; ;Formula: ;speed = profile_speed / cos (altitude) ; ;Parameters r16 0 = not aligned, copy values ; 1 = aligned, calculate formula ;***************************************************************************************** .CSEG mot_calc_az_speeds: .EQU mot_cas_factor = 1 ; .EQU mot_cas_maxspeed = 9 .EQU mot_cas_lspace = 17 LOCAL mot_cas_lspace clr r15 cpse r0 ,r16 ;aligned (r16 = 0) ? rjmp mot_cas_aligned ;yes (r16 <> 0) mot_cas_not_aligned: lds r0 ,mot_a_ssp1_raw sts mot_a_ssp1 ,r0 lds r0 ,mot_a_ssp1_raw+1 sts mot_a_ssp1+1 ,r0 lds r0 ,mot_a_ssp1_raw+2 sts mot_a_ssp1+2 ,r0 lds r0 ,mot_a_ssp1_raw+3 sts mot_a_ssp1+3 ,r0 lds r0 ,mot_a_ssp2_raw sts mot_a_ssp2 ,r0 lds r0 ,mot_a_ssp2_raw+1 sts mot_a_ssp2+1 ,r0 lds r0 ,mot_a_ssp2_raw+2 sts mot_a_ssp2+2 ,r0 lds r0 ,mot_a_ssp2_raw+3 sts mot_a_ssp2+3 ,r0 lds r0 ,mot_a_ssp3_raw sts mot_a_ssp3 ,r0 lds r0 ,mot_a_ssp3_raw+1 sts mot_a_ssp3+1 ,r0 lds r0 ,mot_a_ssp3_raw+2 sts mot_a_ssp3+2 ,r0 lds r0 ,mot_a_ssp3_raw+3 sts mot_a_ssp3+3 ,r0 lds r0 ,mot_a_ssp4_raw sts mot_a_ssp4 ,r0 lds r0 ,mot_a_ssp4_raw+1 sts mot_a_ssp4+1 ,r0 lds r0 ,mot_a_ssp4_raw+2 sts mot_a_ssp4+2 ,r0 lds r0 ,mot_a_ssp4_raw+3 sts mot_a_ssp4+3 ,r0 rjmp mot_cas_end mot_cas_aligned: f_load cor_alt_rad f_push f_const mot__cas_maxalt f_y_lt_x brts mot_cas_00 f_xy mot_cas_00: f_pop f_cos f_push f_const flt__1 f_xy f_div f_store_l mot_cas_factor lds AKKU_2 ,mot_a_ssp5 lds AKKU_3 ,mot_a_ssp5+1 lds AKKU_4 ,mot_a_ssp5+2 lds AKKU_5 ,mot_a_ssp5+3 call flt_convert_long f_store_l mot_cas_maxspeed ;speed setpoint 1 lds AKKU_2 ,mot_a_ssp1_raw lds AKKU_3 ,mot_a_ssp1_raw+1 lds AKKU_4 ,mot_a_ssp1_raw+2 lds AKKU_5 ,mot_a_ssp1_raw+3 call flt_convert_long f_push f_load_l mot_cas_factor f_mul f_push f_load_l mot_cas_maxspeed f_y_le_x ;speed <= maximum brts mot_cas_01 ;yes f_xy ;if not, swap X,Y mot_cas_01: f_pop ;Y into Akku call flt_double2long sts mot_a_ssp1 ,AKKU_2 sts mot_a_ssp1+1 ,AKKU_3 sts mot_a_ssp1+2 ,AKKU_4 sts mot_a_ssp1+3 ,AKKU_5 ;speed setpoint 2 lds AKKU_2 ,mot_a_ssp2_raw lds AKKU_3 ,mot_a_ssp2_raw+1 lds AKKU_4 ,mot_a_ssp2_raw+2 lds AKKU_5 ,mot_a_ssp2_raw+3 call flt_convert_long f_push f_load_l mot_cas_factor f_mul f_push f_load_l mot_cas_maxspeed f_y_le_x ;speed <= maximum brts mot_cas_02 ;yes f_xy ;if not, swap X,Y mot_cas_02: f_pop ;Y into Akku call flt_double2long sts mot_a_ssp2 ,AKKU_2 sts mot_a_ssp2+1 ,AKKU_3 sts mot_a_ssp2+2 ,AKKU_4 sts mot_a_ssp2+3 ,AKKU_5 ;speed setpoint 3 lds AKKU_2 ,mot_a_ssp3_raw lds AKKU_3 ,mot_a_ssp3_raw+1 lds AKKU_4 ,mot_a_ssp3_raw+2 lds AKKU_5 ,mot_a_ssp3_raw+3 call flt_convert_long f_push f_load_l mot_cas_factor f_mul f_push f_load_l mot_cas_maxspeed f_y_le_x ;speed <= maximum brts mot_cas_03 ;yes f_xy ;if not, swap X,Y mot_cas_03: f_pop ;Y into Akku call flt_double2long sts mot_a_ssp3 ,AKKU_2 sts mot_a_ssp3+1 ,AKKU_3 sts mot_a_ssp3+2 ,AKKU_4 sts mot_a_ssp3+3 ,AKKU_5 ;speed setpoint 4 lds AKKU_2 ,mot_a_ssp4_raw lds AKKU_3 ,mot_a_ssp4_raw+1 lds AKKU_4 ,mot_a_ssp4_raw+2 lds AKKU_5 ,mot_a_ssp4_raw+3 call flt_convert_long f_push f_load_l mot_cas_factor f_mul f_push f_load_l mot_cas_maxspeed f_y_le_x ;speed <= maximum brts mot_cas_04 ;yes f_xy ;if not, swap X,Y mot_cas_04: f_pop ;Y into Akku call flt_double2long sts mot_a_ssp4 ,AKKU_2 sts mot_a_ssp4+1 ,AKKU_3 sts mot_a_ssp4+2 ,AKKU_4 sts mot_a_ssp4+3 ,AKKU_5 mot_cas_end: ENDLOCAL mot_cas_lspace ret mot__cas_maxalt: .db 0x3F,0xF8,0x93,0x01,0x1F,0x42,0x6E,0x17 ;1.535889742 (88 degrees) ;***************************************************************************************** ; Stepper Motor Calculations ; called from the keypad routine ; ;Parameters yh:yl adress of motor data block .CSEG mot_speed_null: clr r16 clr r17 clr r18 clr r19 rjmp mot_speed mot_speed_plus5: ldd r16 ,Y+MOT_SSP5 ldd r17 ,Y+MOT_SSP5+1 ldd r18 ,Y+MOT_SSP5+2 ldd r19 ,Y+MOT_SSP5+3 rjmp mot_speed mot_speed_minus5: clr r16 clr r17 clr r18 clr r19 ldd r20 ,Y+MOT_SSP5+3 sub r19 ,r20 ldd r20 ,Y+MOT_SSP5+2 sbc r18 ,r20 ldd r20 ,Y+MOT_SSP5+1 sbc r17 ,r20 ldd r20 ,Y+MOT_SSP5 sbc r16 ,r20 rjmp mot_speed mot_speed_plus4: ldd r16 ,Y+MOT_SSP4 ldd r17 ,Y+MOT_SSP4+1 ldd r18 ,Y+MOT_SSP4+2 ldd r19 ,Y+MOT_SSP4+3 rjmp mot_speed mot_speed_minus4: clr r16 clr r17 clr r18 clr r19 ldd r20 ,Y+MOT_SSP4+3 sub r19 ,r20 ldd r20 ,Y+MOT_SSP4+2 sbc r18 ,r20 ldd r20 ,Y+MOT_SSP4+1 sbc r17 ,r20 ldd r20 ,Y+MOT_SSP4 sbc r16 ,r20 rjmp mot_speed mot_speed_plus3: ldd r16 ,Y+MOT_SSP3 ldd r17 ,Y+MOT_SSP3+1 ldd r18 ,Y+MOT_SSP3+2 ldd r19 ,Y+MOT_SSP3+3 rjmp mot_speed mot_speed_minus3: clr r16 clr r17 clr r18 clr r19 ldd r20 ,Y+MOT_SSP3+3 sub r19 ,r20 ldd r20 ,Y+MOT_SSP3+2 sbc r18 ,r20 ldd r20 ,Y+MOT_SSP3+1 sbc r17 ,r20 ldd r20 ,Y+MOT_SSP3 sbc r16 ,r20 rjmp mot_speed mot_speed_plus2: ldd r16 ,Y+MOT_SSP2 ldd r17 ,Y+MOT_SSP2+1 ldd r18 ,Y+MOT_SSP2+2 ldd r19 ,Y+MOT_SSP2+3 rjmp mot_speed mot_speed_minus2: clr r16 clr r17 clr r18 clr r19 ldd r20 ,Y+MOT_SSP2+3 sub r19 ,r20 ldd r20 ,Y+MOT_SSP2+2 sbc r18 ,r20 ldd r20 ,Y+MOT_SSP2+1 sbc r17 ,r20 ldd r20 ,Y+MOT_SSP2 sbc r16 ,r20 rjmp mot_speed mot_speed_plus1: ldd r16 ,Y+MOT_SSP1 ldd r17 ,Y+MOT_SSP1+1 ldd r18 ,Y+MOT_SSP1+2 ldd r19 ,Y+MOT_SSP1+3 rjmp mot_speed mot_speed_minus1: clr r16 clr r17 clr r18 clr r19 ldd r20 ,Y+MOT_SSP1+3 sub r19 ,r20 ldd r20 ,Y+MOT_SSP1+2 sbc r18 ,r20 ldd r20 ,Y+MOT_SSP1+1 sbc r17 ,r20 ldd r20 ,Y+MOT_SSP1 sbc r16 ,r20 rjmp mot_speed mot_speed: cli ;new speed setpoint std Y+MOT_HSSP ,r16 std Y+MOT_HSSP+1 ,r17 std Y+MOT_HSSP+2 ,r18 std Y+MOT_HSSP+3 ,r19 sei ret mot_acc_plus: ret mot_acc_minus: ret ;***************************************************************************************** ; Stepper motor speed calculation ; sp = hsp + gsp (limited between -ssp5 and +ssp5) ; ;yh:yl adress of motor data block ;***************************************************************************************** .CSEG mot_calc_speed: ldd r2 ,Y+MOT_HSP ;handbox speed ldd r3 ,Y+MOT_HSP+1 ldd r4 ,Y+MOT_HSP+2 ldd r5 ,Y+MOT_HSP+3 ldd r6 ,Y+MOT_GSP ;tracking speed ldd r7 ,Y+MOT_GSP+1 ldd r8 ,Y+MOT_GSP+2 ldd r9 ,Y+MOT_GSP+3 add r5 ,r9 adc r4 ,r8 adc r3 ,r7 adc r2 ,r6 cli std Y+MOT_SP ,r2 std Y+MOT_SP+1 ,r3 std Y+MOT_SP+2 ,r4 std Y+MOT_SP+3 ,r5 sei ret ;***************************************************************************************** ; Stepper motor ramp calculation ;yh:yl adress of motor data block ;Y speed setpoint ;Y+4 speed ;Y+8 acceleration ;***************************************************************************************** .CSEG mot_ramp: cli ldd r6 ,Y+0 ;speed setpoint ldd r7 ,Y+0+1 ldd r8 ,Y+0+2 ldd r9 ,Y+0+3 sei cli ldd r2 ,Y+4 ;speed ldd r3 ,Y+4+1 ldd r4 ,Y+4+2 ldd r5 ,Y+4+3 sei cli ldd r10 ,Y+8 ;acceleration ldd r11 ,Y+8+1 ldd r12 ,Y+8+2 ldd r13 ,Y+8+3 sei mov r16 ,r2 ;offset 0x80 for high byte of speed and setpoint subi r16 ,0x80 ;for simpler comparisons mov r17 ,r6 subi r17 ,0x80 cp r5 ,r9 cpc r4 ,r8 cpc r3 ,r7 cpc r16 ,r17 breq mot_ramp_end ;speed == setpoint, nothing to do brcc mot_ramp_minus ;speed > setpoint mot_ramp_plus: add r5 ,r13 ;speed += acceleration adc r4 ,r12 adc r3 ,r11 adc r2 ,r10 mov r16 ,r2 subi r16 ,0x80 cp r9 ,r5 ;too fast (speed > setpoint) cpc r8 ,r4 cpc r7 ,r3 cpc r17 ,r16 brcs mot_ramp_equal ;yes, set speed := setpoint rjmp mot_ramp_end mot_ramp_minus: sub r5 ,r13 ;speed -= acceleration sbc r4 ,r12 sbc r3 ,r11 sbc r2 ,r10 mov r16 ,r2 subi r16 ,0x80 cp r5 ,r9 ;too slow (speed < setpoint) cpc r4 ,r8 cpc r3 ,r7 cpc r16 ,r17 brcs mot_ramp_equal ;yes, set speed := setpoint rjmp mot_ramp_end mot_ramp_equal: mov r2 ,r6 mov r3 ,r7 mov r4 ,r8 mov r5 ,r9 mot_ramp_end: cli std Y+4 ,r2 std Y+4+1 ,r3 std Y+4+2 ,r4 std Y+4+3 ,r5 sei mov r16 ,r2 ;test for zero speed or r16 ,r3 or r16 ,r4 or r16 ,r5 ret ;***************************************************************************************** ; Stepper Motor Initialization ; High Level ;***************************************************************************************** .CSEG mot_init: clr r0 ; Motor A ldi yl ,low(mot_a) ldi yh ,high(mot_a) std Y+MOT_POS ,r0 ;position std Y+MOT_POS+1 ,r0 std Y+MOT_POS+2 ,r0 std Y+MOT_POS+3 ,r0 std Y+MOT_POS+4 ,r0 std Y+MOT_POS+5 ,r0 std Y+MOT_SP ,r0 ;speed std Y+MOT_SP+1 ,r0 std Y+MOT_SP+2 ,r0 std Y+MOT_SP+3 ,r0 std Y+MOT_GSSP ,r0 ;tracking/goto speed std Y+MOT_GSSP+1 ,r0 std Y+MOT_GSSP+2 ,r0 std Y+MOT_GSSP+3 ,r0 std Y+MOT_GSP ,r0 std Y+MOT_GSP+1 ,r0 std Y+MOT_GSP+2 ,r0 std Y+MOT_GSP+3 ,r0 std Y+MOT_HSSP ,r0 ;handbox speed std Y+MOT_HSSP+1 ,r0 std Y+MOT_HSSP+2 ,r0 std Y+MOT_HSSP+3 ,r0 std Y+MOT_HSP ,r0 std Y+MOT_HSP+1 ,r0 std Y+MOT_HSP+2 ,r0 std Y+MOT_HSP+3 ,r0 ; Motor B ldi yl ,low(mot_b) ldi yh ,high(mot_b) std Y+MOT_POS ,r0 ;position std Y+MOT_POS+1 ,r0 std Y+MOT_POS+2 ,r0 std Y+MOT_POS+3 ,r0 std Y+MOT_POS+4 ,r0 std Y+MOT_POS+5 ,r0 std Y+MOT_SP ,r0 ;speed std Y+MOT_SP+1 ,r0 std Y+MOT_SP+2 ,r0 std Y+MOT_SP+3 ,r0 std Y+MOT_GSSP ,r0 ;tracking/goto speed std Y+MOT_GSSP+1 ,r0 std Y+MOT_GSSP+2 ,r0 std Y+MOT_GSSP+3 ,r0 std Y+MOT_GSP ,r0 std Y+MOT_GSP+1 ,r0 std Y+MOT_GSP+2 ,r0 std Y+MOT_GSP+3 ,r0 std Y+MOT_HSSP ,r0 ;handbox speed std Y+MOT_HSSP+1 ,r0 std Y+MOT_HSSP+2 ,r0 std Y+MOT_HSSP+3 ,r0 std Y+MOT_HSP ,r0 std Y+MOT_HSP+1 ,r0 std Y+MOT_HSP+2 ,r0 std Y+MOT_HSP+3 ,r0 ret ;*****************************************************************************************