📄 map_bot.asm
字号:
; // Copy servo data
; servo_pos[0] = recv_buffer[4];
m007 BCF 0x03,RP0
BCF 0x03,RP1
MOVF recv_buffer+4,W
MOVWF servo_pos
; servo_pos[1] = recv_buffer[5];
MOVF recv_buffer+5,W
MOVWF servo_pos+1
; servo_pos[2] = recv_buffer[6];
MOVF recv_buffer+6,W
MOVWF servo_pos+2
; servo_pos[3] = recv_buffer[7];
MOVF recv_buffer+7,W
MOVWF servo_pos+3
; servo_pos[4] = recv_buffer[8];
MOVF recv_buffer+8,W
MOVWF servo_pos+4
; servo_pos[5] = recv_buffer[9];
MOVF recv_buffer+9,W
MOVWF servo_pos+5
;
;
; recv_counter++;
INCF recv_counter,1
;
; if (recv_counter >= 10)
MOVLW .10
SUBWF recv_counter,W
BTFSC 0x03,Carry
; {
; recv_counter = 0;
CLRF recv_counter
; //led_1 = !led_1;
; }
;
;
; last_valid = 1;
BCF 0x03,RP0
BCF 0x03,RP1
BSF 0x64,last_valid
; }
; else
GOTO m009
; last_valid = 0;
m008 BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,last_valid
;
; reading_data = 0;
m009 BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,reading_data
;
; update_timeout = 0; // Reset update timeout
CLRF update_timeout
;
;
; }
; else
GOTO m011
; recv_buffer[cur_recv_byte++] = RCREG; // Normal char for packet
m010 MOVLW .36
BCF 0x03,RP0
BCF 0x03,RP1
ADDWF cur_recv_byte,W
MOVWF FSR
BCF 0x03,IRP
MOVF RCREG,W
MOVWF INDF
INCF cur_recv_byte,1
; }
;
; // Ready for TX byte
; if (TXIF && TXIE)
m011 BCF 0x03,RP0
BCF 0x03,RP1
BTFSS 0x0C,TXIF
GOTO m013
BSF 0x03,RP0
BTFSS 0x8C,TXIE
GOTO m013
; {
; // End of packet?
; if(cur_send_byte >= NUM_SEND_BYTES)
MOVLW .20
BCF 0x03,RP0
SUBWF cur_send_byte,W
BTFSS 0x03,Carry
GOTO m012
; {
; cur_send_byte = 0; // Reset to first byte for next send
CLRF cur_send_byte
; TXIE = 0; // Last byte has been sent, disable tx
BSF 0x03,RP0
BCF 0x8C,TXIE
; }
; else
GOTO m013
; TXREG = send_buffer[cur_send_byte++]; // Send next byte
m012 MOVLW .47
BCF 0x03,RP0
BCF 0x03,RP1
ADDWF cur_send_byte,W
MOVWF FSR
BCF 0x03,IRP
MOVF INDF,W
MOVWF TXREG
INCF cur_send_byte,1
; }
;
;
; // check for timer zero interrupt
; if (T0IF)
m013 BTFSS 0x0B,T0IF
GOTO m021
; {
;
; if (servo_mode == 0) // setupup for base pos wait
BCF 0x03,RP0
BCF 0x03,RP1
MOVF servo_mode,1
BTFSS 0x03,Zero_
GOTO m016
; {
; servo_mode = 1; // next trigger we will setup for base delay
MOVLW .1
MOVWF servo_mode
; OPTION = 4; // reset back into 1:32 prescaled mode
MOVLW .4
BSF 0x03,RP0
MOVWF OPTION_REG
;
; TMR0 = 0xa0; // set base time delay
MOVLW .160
BCF 0x03,RP0
MOVWF TMR0
;
; unsigned char s = 1 << cur_servo; // create mask
MOVLW .1
MOVWF s
MOVF cur_servo,W
BTFSC 0x03,Zero_
GOTO m015
MOVWF C1cnt
m014 BCF 0x03,Carry
BCF 0x03,RP0
BCF 0x03,RP1
RLF s,1
DECFSZ C1cnt,1
GOTO m014
;
; PORTC |= s; // turn this one on
m015 BCF 0x03,RP0
BCF 0x03,RP1
MOVF s,W
IORWF PORTC,1
;
; } else
GOTO m020
; if (servo_mode == 1) // setup for position hold
m016 BCF 0x03,RP0
BCF 0x03,RP1
DECFSZ servo_mode,W
GOTO m017
; {
; servo_mode = 2; // next mode will turn off and check for long delay
MOVLW .2
MOVWF servo_mode
; TMR0 = servo_pos[cur_servo]; // reset the timer to the pos
MOVLW .67
ADDWF cur_servo,W
MOVWF FSR
BCF 0x03,IRP
MOVF INDF,W
MOVWF TMR0
;
; } else
GOTO m020
; if (servo_mode == 2) // setupfor next servo
m017 BCF 0x03,RP0
BCF 0x03,RP1
MOVF servo_mode,W
XORLW .2
BTFSS 0x03,Zero_
GOTO m020
; {
; servo_mode = 0; // reset back to base mode
CLRF servo_mode
;
; unsigned char s = 1 << cur_servo; // create mask
MOVLW .1
MOVWF s_2
MOVF cur_servo,W
BTFSC 0x03,Zero_
GOTO m019
MOVWF C2cnt
m018 BCF 0x03,Carry
BCF 0x03,RP0
BCF 0x03,RP1
RLF s_2,1
DECFSZ C2cnt,1
GOTO m018
;
; PORTC &= ~s; // turn the current servo off
m019 BCF 0x03,RP0
BCF 0x03,RP1
COMF s_2,W
ANDWF PORTC,1
;
; cur_servo++; // next servo
INCF cur_servo,1
;
; // Setup longer delay to fill the 20 ms window,
; // we need to burn 11ms on average
; // 5Mhz clock, 0.0512 MS per increment at 1:256 prescaler
; // 11ms / 0.0512 ms = 215 cycles to burn, start time at 40
;
; if (cur_servo == NUM_SERVOS)
MOVF cur_servo,W
XORLW .6
BTFSS 0x03,Zero_
GOTO m020
; {
; cur_servo = 0; // reset back the first servo
CLRF cur_servo
;
; OPTION = 7;
MOVLW .7
BSF 0x03,RP0
MOVWF OPTION_REG
;
; TMR0 = 40; // set the value for the longer delay
MOVLW .40
BCF 0x03,RP0
MOVWF TMR0
; }
; }
;
; T0IF = 0; // clear timer zero interrupt
m020 BCF 0x0B,T0IF
; }
;
; // check for timer one interrupt
; if (TMR1IF)
m021 BCF 0x03,RP0
BCF 0x03,RP1
BTFSS 0x0C,TMR1IF
GOTO m026
; {
; TMR1L = 0x00; // reset timer
CLRF TMR1L
; TMR1H = 0x80;
MOVLW .128
MOVWF TMR1H
;
; running_counter++;
INCF running_counter,1
;
;
;
;
;
; if (running_counter >= 20)
MOVLW .20
SUBWF running_counter,W
BTFSS 0x03,Carry
GOTO m022
; {
;run_count++;
INCF run_count,1
; encoder_0_speed = encoder_0_count;
MOVF encoder_0_count,W
MOVWF encoder_0_speed
; encoder_0_count = 0;
CLRF encoder_0_count
; led_1 = !led_1;
MOVLW .128
XORWF PORTB,1
; running_counter = 0;
CLRF running_counter
; // led_0 = !led_0;
; }
;
; if (reading_data)
m022 BCF 0x03,RP0
BCF 0x03,RP1
BTFSS 0x64,reading_data
GOTO m023
; {
; read_timeout++;
INCF read_timeout,1
;
; // The read has timed out
; if (read_timeout >= 4)
MOVLW .4
SUBWF read_timeout,W
BTFSS 0x03,Carry
GOTO m023
; {
; reading_data = 0; // reset reading
BCF 0x64,reading_data
; cur_recv_byte = 0; // reset to current byte
CLRF cur_recv_byte
; last_valid = 1;
BSF 0x64,last_valid
; }
; }
;
;
; // Update time out
; update_timeout++;
m023 BCF 0x03,RP0
BCF 0x03,RP1
INCF update_timeout,1
;
; // Too many timeouts have passed, kill motors
; if (update_timeout >= 30)
MOVLW .30
SUBWF update_timeout,W
BTFSS 0x03,Carry
GOTO m024
; {
; update_timeout = 30;
MOVLW .30
MOVWF update_timeout
; pwm_value[0] = 0;
CLRF pwm_value
; pwm_value[1] = 0;
CLRF pwm_value+1
; pwm_value[2] = 0;
CLRF pwm_value+2
; pwm_value[3] = 0;
CLRF pwm_value+3
; }
;
;
; // Check for recevive error
; if (OERR)
m024 BCF 0x03,RP0
BCF 0x03,RP1
BTFSS 0x18,OERR
GOTO m025
; {
; CREN = 0; // reset recieve logic
BCF 0x18,CREN
; CREN = 1;
BSF 0x18,CREN
; }
;
;
;
; TMR1IF = 0; // clear flag
m025 BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x0C,TMR1IF
; }
;
; if (TMR2IF)
m026 BCF 0x03,RP0
BCF 0x03,RP1
BTFSS 0x0C,TMR2IF
GOTO m044
; {
;// led_0 = encoder_0;
;
; if (encoder_0 != last_encoder_0)
BTFSC 0x08,encoder_0
GOTO m027
BTFSC 0x64,last_encoder_0
GOTO m028
GOTO m029
m027 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x64,last_encoder_0
GOTO m029
; {
; encoder_low[0]++;
m028 INCF encoder_low,1
;
; if (encoder_low[0] == 0)
BTFSC 0x03,Zero_
; encoder_high[0]++;
INCF encoder_high,1
;
;
; last_encoder_0 = encoder_0;
BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,last_encoder_0
BTFSC 0x08,encoder_0
BSF 0x64,last_encoder_0
; encoder_0_count++;
INCF encoder_0_count,1
; }
;
; if (encoder_1 != last_encoder_1)
m029 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x08,encoder_1
GOTO m030
BTFSC 0x64,last_encoder_1
GOTO m031
GOTO m032
m030 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x64,last_encoder_1
GOTO m032
; {
; encoder_low[1]++;
m031 INCF encoder_low+1,1
;
; if (encoder_low[1] == 0)
BTFSC 0x03,Zero_
; encoder_high[1]++;
INCF encoder_high+1,1
;
; last_encoder_1 = encoder_1;
BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,last_encoder_1
BTFSC 0x08,encoder_1
BSF 0x64,last_encoder_1
; encoder_1_count++;
INCF encoder_1_count,1
; }
;
;
; if (encoder_2 != last_encoder_2)
m032 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x08,encoder_2
GOTO m033
BTFSC 0x64,last_encoder_2
GOTO m034
GOTO m035
m033 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x64,last_encoder_2
GOTO m035
; {
; encoder_low[2]++;
m034 INCF encoder_low+2,1
;
; if (encoder_low[2] == 0)
BTFSC 0x03,Zero_
; encoder_high[2]++;
INCF encoder_high+2,1
;
;
; last_encoder_2 = encoder_2;
BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,last_encoder_2
BTFSC 0x08,encoder_2
BSF 0x64,last_encoder_2
; encoder_2_count++;
INCF encoder_2_count,1
; }
;
; if (encoder_3 != last_encoder_3)
m035 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x08,encoder_3
GOTO m036
BTFSC 0x64,last_encoder_3
GOTO m037
GOTO m038
m036 BCF 0x03,RP0
BCF 0x03,RP1
BTFSC 0x64,last_encoder_3
GOTO m038
; {
; encoder_low[3]++;
m037 INCF encoder_low+3,1
;
; if (encoder_low[3] == 0)
BTFSC 0x03,Zero_
; encoder_high[3]++;
INCF encoder_high+3,1
;
; last_encoder_3 = encoder_3;
BCF 0x03,RP0
BCF 0x03,RP1
BCF 0x64,last_encoder_3
BTFSC 0x08,encoder_3
BSF 0x64,last_encoder_3
; encoder_3_count++;
INCF encoder_3_count,1
; }
;
; pwm_cycle++;
m038 BCF 0x03,RP0
BCF 0x03,RP1
INCFSZ pwm_cycle,1
;
; // Check for cycle starts.
; // Do not set the pin if the value is zero
; if (pwm_cycle == 0 && pwm_value[0]) pwm_out_0 = 1;
GOTO m039
MOVF pwm_value,1
BTFSS 0x03,Zero_
BSF 0x06,pwm_out_0
; if (pwm_cycle == 0 && pwm_value[1]) pwm_out_1 = 1;
m039 BCF 0x03,RP0
BCF 0x03,RP1
MOVF pwm_cycle,1
BTFSS 0x03,Zero_
GOTO m040
MOVF pwm_value+1,1
BTFSS 0x03,Zero_
BSF 0x06,pwm_out_1
; if (pwm_cycle == 0 && pwm_value[2]) pwm_out_2 = 1;
m040 BCF 0x03,RP0
BCF 0x03,RP1
MOVF pwm_cycle,1
BTFSS 0x03,Zero_
GOTO m041
MOVF pwm_value+2,1
BTFSS 0x03,Zero_
BSF 0x06,pwm_out_2
; if (pwm_cycle == 0 && pwm_value[3]) pwm_out_3 = 1;
m041 BCF 0x03,RP0
BCF 0x03,RP1
MOVF pwm_cycle,1
BTFSS 0x03,Zero_
GOTO m042
MOVF pwm_value+3,1
BTFSS 0x03,Zero_
BSF 0x06,pwm_out_3
;
; // turn outputs off after their duty has passed
; if (pwm_cycle > pwm_value[0]) pwm_out_0 = 0;
m042 BCF 0x03,RP0
BCF 0x03,RP1
MOVF pwm_cycle,W
SUBWF pwm_value,W
BTFSS 0x03,Carry
BCF 0x06,pwm_out_0
; if (pwm_cycle > pwm_value[1]) pwm_out_1 = 0;
BCF 0x03,RP0
BCF 0x03,RP1
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -