; part 2
; **************************
; Main
; **************************
Main:
Loop:
ldi r16,POTPIN ; // read the user input (CW/CCW) potentiometer
rcall analogRead ; PotValue = analogRead(PotPin);
mov PotValue,r16
IF1:
cpi PotValue,CW_TRESHOLD ; // if CW requested, then go to RUN mode in the CW direction
brlo ELSEIF1 ; if (PotValue >= CW_TRESHOLD) {
ldi TargetDirection,CW ; TargetDirection = CW;
ldi Mode,RUN ; Mode = RUN;
IF2: ; if (PotValue >= CW_TRESHOLD_LUDICROUS)
cpi PotValue,CW_TRESHOLD_LUDICROUS
brlo ENDIF2
ldi Mode,LUDICROUS ; Mode = LUDICROUS;
ENDIF2:
rjmp ENDIF1 ; }
ELSEIF1: ; // else if CCW, then go to RUN mode in the CCW direction
cpi PotValue,CCW_TRESHOLD ; else if (PotValue < CCW_TRESHOLD)
brsh ENDIF1 ; {
ldi TargetDirection,CCW ; TargetDirection = CCW;
ldi Mode,RUN ; Mode = RUN;
IF3: ; if (PotValue < CCW_TRESHOLD_LUDICROUS)
cpi PotValue,CCW_TRESHOLD_LUDICROUS
brsh ENDIF3
ldi Mode,LUDICROUS ; Mode = LUDICROUS;
ENDIF3:
ENDIF1: ; }
; // do all the mode-specific stuff
;
; switch (Mode) {
cpi Mode,STOP ; case STOP:
brne RUNTEST
ldi TargetSpeed,0 ; TargetSpeed = 0;
rjmp ENDSWITCH ; break;
RUNTEST: ;
cpi Mode,RUN ; case RUN:
brne LUDTEST
ldi TargetSpeed,RUN_SPEED ; TargetSpeed = RUN_SPEED;
IF4:
cpi r16,APPROACH_TRESHOLD_LOW ; // if potentiometer returned to middle, then go to APPROACH mode
brlo ENDIF4 ; if (PotValue >= APPROACH_TRESHOLD_LOW && PotValue < APPROACH_TRESHOLD_HIGH)
cpi r16,APPROACH_TRESHOLD_HIGH
brsh ENDIF4 ; {
ldi Mode,APPROACH ; Mode = APPROACH;
ldi TargetSpeed,APPROACH_SPEED ; TargetSpeed = APPROACH_SPEED;
ENDIF4: ; }
rjmp ENDSWITCH ; break;
LUDTEST:
cpi Mode,LUDICROUS ; case LUDICROUS:
brne APPRTEST
ldi TargetSpeed,LUDICROUS_SPEED ; TargetSpeed = LUDICROUS_SPEED;
IF5:
cpi r16,APPROACH_TRESHOLD_LOW ; // if potentiometer returned to middle, then go to APPROACH mode
brlo ENDIF5 ; if (PotValue >= APPROACH_TRESHOLD_LOW && PotValue < APPROACH_TRESHOLD_HIGH)
cpi r16,APPROACH_TRESHOLD_HIGH ; {
brsh ENDIF5
ldi Mode,APPROACH ; Mode = APPROACH;
ldi TargetSpeed,APPROACH_SPEED ; TargetSpeed = APPROACH_SPEED;
ENDIF5: ; }
rjmp ENDSWITCH ; break;
APPRTEST: ;
cpi Mode,APPROACH ; case APPROACH:
brne HUNTTEST
IF6:
cp Direction,TargetDirection ; // wait until approach speed and direction have stabilized
brne ENDIF6 ; if (Direction == TargetDirection && Speed == TargetSpeed)
cp Speed,TargetSpeed ; {
brne ENDIF6
IF7: ; // read the leading edge phototransistor
cpi Direction,CW ; if (Direction == CW)
brne ELSE7 ; {
ldi r16,PTCW ; LeadingPT = analogRead(PTCW);
rjmp ENDIF7 ; }
ELSE7: ; else
; {
ldi r16,PTCCW ; LeadingPT = analogRead(PTCCW);
ENDIF7: ; }
rcall analogRead
mov LeadingPT,r16
IF8: ; // if occluded, then go to HUNT mode
cpi LeadingPT,HUNT_TRESHOLD ; if (LeadingPT >= HUNT_TRESHOLD)
brlo ENDIF8 ; {
ldi Mode,HUNT ; Mode = HUNT;
ldi TargetSpeed,HUNT_SPEED ; TargetSpeed = HUNT_SPEED;
ENDIF8: ; }
ENDIF6: ; }
rjmp ENDSWITCH ; break;
HUNTTEST: ; case HUNT:
cpi Mode,HUNT
brne ENDSWITCH
ldi r16,PTCW ; // read phototransistors
cpi Direction,CW ; LeadingPT = analogRead(Direction==CW?PTCW:PTCCW);
breq A1
ldi r16,PTCCW
A1: rcall analogRead
mov LeadingPT,r16
ldi r16,PTCCW ; TrailingPT = analogRead(Direction==CW?PTCCW:PTCW);
cpi Direction,CW
breq A2
ldi r16,PTCW
A2: rcall analogRead ;
mov TrailingPT,r16
IF9: ; // if values close enough, then go to STOP mode
ldi r16,BALANCE_TRESHOLD ; if (LeadingPT<TrailingPT+BALANCE_TRESHOLD)
add TrailingPT,r16
cp LeadingPT,TrailingPT
brsh ENDIF9 ; {
ldi Mode,STOP ; Mode = STOP;
ldi Speed,0 ; Speed = 0;
ldi TargetSpeed,0 ; TargetSpeed = 0;
DO1: ; // Fine hunt loop == might not be necessary in the ASM version
; do
; {
ldi r16,PTCW ; LeadingPT = analogRead(Direction==CW?PTCW:PTCCW);
cpi Direction,CW
breq A3
ldi r16,PTCCW
A3: rcall analogRead
mov LeadingPT,r16
ldi r16,PTCCW ; TrailingPT = analogRead(Direction==CW?PTCCW:PTCW);
cpi Direction,CW
breq A4
ldi r16,PTCW
A4: rcall analogRead
mov TrailingPT,r16
ldi r16,FINE_BALANCE_LIMIT
add TrailingPT,r16 ; } while(LeadingPT>=TrailingPT+FINE_BALANCE_LIMIT)
cp LeadingPT,TrailingPT
brsh DO1
ldi r17,1 ; // deliberately overshoot for 0.20ms
ldi r16,51
; rcall dec_loop
rcall Drive ; // perform immediate motor stop
; analogWrite(Motor1, 0);
; analogWrite(Motor2, 0);
ENDIF9: ; }
rjmp ENDSWITCH ; break;
; }
ENDSWITCH:
; // Handle acceleration/deceleration:
; // =================================
;
IF10: ; // if target speed/direction not yet attained, then adjust speed
cp Direction,TargetDirection ; if (Direction != TargetDirection)
breq ELSEIF10 ; {
IF11: ; // Decelerate to 0 speed
cpi Speed,MINIMUM_SPEED ; if (Speed > MINIMUM_SPEED)
brlo ELSE11 ; {
subi Speed,STD_ACCEL ; Speed -= STD_ACCEL;
rjmp ENDIF11 ; }
ELSE11: ; else
; {
; // 0 speed attained, time to start accelerating in the other direction
ldi Speed,0 ; Speed = 0;
mov Direction,TargetDirection ; Direction = TargetDirection;
ENDIF11: ; }
rjmp ENDIF10 ; }
ELSEIF10: ; else if (Speed < TargetSpeed)
cp Speed,TargetSpeed ; {
brsh ELSEIF10b
IF12: ; // Accelerate
cpi Speed,MINIMUM_SPEED ; if (Speed < MINIMUM_SPEED)
brsh ELSEIF12
ldi Speed,MINIMUM_SPEED ; Speed = MINIMUM_SPEED; // jump straight to MINIMUM_SPEED
rjmp ENDIF12
ELSEIF12: ; else
ldi r16,STD_ACCEL ; Speed += STD_ACCEL;
add Speed,r16
ENDIF12:
rjmp ENDIF10 ; }
ELSEIF10b: ; else if (Speed >= TargetSpeed+1 && Speed >= MINIMUM_SPEED+1)
cpi Speed,MINIMUM_SPEED+1 ; {
brlo ENDIF10
mov r16,TargetSpeed
inc r16
cp Speed,r16
brlo ENDIF10
; // Decelerate
subi Speed,STD_ACCEL ; Speed -= STD_ACCEL;
ENDIF10: ; }
rcall Drive ; // Update motor speed
; if (Direction == CW)
; {
; analogWrite(Motor1, Speed);
; analogWrite(Motor2, 0);
; }
; else
; {
; analogWrite(Motor1, 0);
; analogWrite(Motor2, Speed);
; }
IF13: ; // Delay 50 ms (Control program runs 20 times per second), except only 10ms in HUNT mode
cpi Mode,HUNT ; if (Mode == HUNT)
brne ELSE13 ; {
ldi r17,5 ; delay (10);
rjmp ENDIF13 ; }
ELSE13: ; else
; {
ldi r17,10 ; delay(25);
ENDIF13: ; }
rcall delay
rjmp Loop ; }