'---------------Title--------------

' File......4331_encdr4_wo.pbp
' Started....1/10/10

' Microcontroller Used:  Microchip Technology 18F4331
'   Available at: 
'    http://www.microchipdirect.com/ProductDetails.aspx?Category=PIC18F4331
'    or http://www.digikey.com/
' Motor Controller Used: Xavien 2 Motor Driver "XDDCMD-1
'   Available at: http://encodergeek.com/Xavien_Amplifier.html
' Motor and Encoder Used: Small Motor with Quadrature Incremental Encoder
'   Available at: http://encodergeek.com/DCMtr_SMALL.html
'
' PicBasic Pro Code: micro-Engineering Labs, Inc.
'                    melabs.com 
 
'--------Program Desciption--------

' Program ramps up the motor power to full power and 
' then slows the motor down as it approaches target position
' (Diff = 0). If the starting position is close to the target,
' the motor will ramp-up then ramp-down power without
' necessarily reaching full power.  
' This program is comment free.


    DEFINE LCD_DREG PORTD   
    DEFINE LCD_DBIT 4       
    define LCD_BITS 4       
    DEFINE LCD_RSREG PORTE  
    DEFINE LCD_RSBIT 0      
    DEFINE LCD_EREG PORTE   
    DEFINE LCD_EBIT 1       
    DEFINE LCD_LINES 2      
    DEFINE LCD_COMMANDUS 2000 
    DEFINE LCD_DATAUS 50     
    DEFINE ADC_BITS 8       
    DEFINE ADC_CLOCK 3      
    DEFINE ADC_SAMPLEUS 50  
    DEFINE CCP2_REG PORTC   
    DEFINE CCP2_BIT 1       
                                          
'------------Variables-------------

    target         VAR WORD 
    mot_pwr        var word       
    position       var word
    diff           var word
    diff_start     var word
         
'----------Initialization----------
    
    CCP1CON = %00111111         
    ANSEL0 = %00000000          
    ANSEL1 = %00000000              
    TRISA = %00011111           
    LATA  = %00000000           
    TRISB = %00000000           
    TRISC = %00000000           
    QEICON = %10001000          
    PORTC.0 = 1                
    PORTC.1 = 0                 
                                
'-------------Main Code------------ 

    pause 500                   
    PORTC.0 = 0                 
    target = 32000              
    
    POSCNTH = 127               
    POSCNTL = 0                 
    
    gosub choose_ramp_up

loop:

    gosub set_motor_direction 
    gosub full_pwr_and_ramp_down    
    GOTO loop                       
    end
     
choose_ramp_up:                 
       
    position = 256 * POSCNTH + POSCNTL  
    if target >= position then  
    diff_start = target - position
    else
    diff_start = position - target
    endif                                       
         
    if diff_start >= 360 then   
        gosub full_ramp_up      
    else                        
        gosub short_ramp_up      
    endif  
    return                      
    
full_ramp_up:                   

    gosub set_motor_direction   
    gosub calculate_diff        
    mot_pwr = (diff_start - diff) + 75
                                      
    if mot_pwr < 255 then       
         gosub lcd              
                                  
         goto full_ramp_up      
    else                          
         mot_pwr = 255          
         gosub lcd              
    endif
    return                     
       
short_ramp_up:                 
                                
    gosub set_motor_direction   
    gosub calculate_diff        
    mot_pwr = (diff_start - diff) + 75
                                    
    if (diff_start)/2 <= diff then
                                
        gosub lcd               
                                
        goto short_ramp_up      
    else                        
    endif
    return                      
    
set_motor_direction:            
     
    position = 256 * POSCNTH + POSCNTL  
    if target < position then   
    PORTC.3 = 1                 
    else
    PORTC.3 = 0                 
    endif  
    return 
        
full_pwr_and_ramp_down:

    gosub calculate_diff              
    select case diff            
        case is = 0             
            PORTC.0 = 1          
            gosub lcd           
        case is > 180           
            PORTC.0 = 0         
            mot_pwr = 255       
            gosub lcd           
        case is <= 180          
            PORTC.0 = 0         
            mot_pwr = diff + 75 
            gosub lcd           
    end select 
    return

calculate_diff:

    position = 256 * POSCNTH + POSCNTL  
    if target >= position then  
    diff = target - position
    else
    diff = position - target
    endif
    return    
        
lcd: 
    
    HPWM 2, mot_pwr, 20000      
    LCDOUT $FE, $80, "Pwr=",dec3 mot_pwr," Df=",DEC5 diff      
    LCDOUT $FE, $C0, "T=",DEC5 target," Ps=", DEc5 position 
    return