ARM Assembler - Loop getting stuck

I'm currently writing some basic ARM assembler. The idea is to control a theoretical furnace and it's cooling/heating system on the LPC2378 development board with an application board attached. I'm getting there as far as recognising input from the devices is concerned. However, i'm having trouble getting my loop right and I was hoping you guys may be able to point out why my loop is getting stuck, and even after button 1 has been pressed, the fan is not turning on. I previously had the fan working, but when I started trying to control the fan by the temperature of the heater it does not seem to ever start up, and the heater stays on, getting hotter and hotter without the fan to cool it down. I'm guessing it could come down to the way i'm reading the temperature from the ADC controller. Please don't hesitate to ask if you need any more information to answer the question. ``` ;========================================================================= ; MotorControl_1 ; ; BUT1 turns motor ON and BUT2 turns motor OFF ; ; No interrupt support (except for Reset) - as simple as it gets! ; ; WDH, November 2008 ;========================================================================== ; Set-up interrupt vectors and stack support $ LPC2378InterruptVectors.s ; After a reset exception, execution starts here - the processor is in ARM ; mode and supervisor state with interrupts disabled SECTION .text:CODE:NOROOT(2) REQUIRE __vector ; Forces reference to this symbol - required by linker ARM ; __iar_program_start - defined start symbol. __iar_program_start: ; Include lpc2378 IO register definitions #include "iolpc2378.h" ; Include Olimex LPC-2378-STK IO definitions #include "OlimexLPC2378BoardDefs.h" ; Main entry point - required by debugger ;========================================================================= ; main starts here ;========================================================================= main ;========================================================================= ; Stack setup ; ; Stack pointer (r13 or SP) is assigned the highest address in a 32kB ; section of on-chip SRAM - assumes a FULL descending stack convention ;========================================================================= ldr r13, = 0x40008000 ldr r2, = 0x000050 ;Load the delay value ldr r3, = 0x028 ;Start set point at 40 ldr r4, = 0x000 ;Start temp difference as 0 ; ldr r6, = 0x0f ;Temp High Value (15) ; ldr r7, = -0xF ;Set r7 to -15. ; Initialise IO BL InitialiseARM_IO BL InitialiseApplicationsBoard_IO ; Initialise the devices and display BL FanOff BL errorLedOff BL systemLedOff BL heaterOff LoopStart BL WaitBUT1 BL heaterOn BL systemLedOn BL readTemp BL checkTemp CMP r0, #5 BGT errorVal SUBS r7, r6, r4 ;Performs r7 = r6 - r4 and sets condition register BLT LoopStart ;Branches to label_bar if r7 < 0 (in which case r6 < r4) BGT heaterOff BGT errorLedOn BGT FanOn BGT LoopStart BL WaitBUT2 BL FanOff BL errorLedOff BL systemLedOff BL heaterOff B LoopStart ;========================================================================= ; InitialiseARM_IO ; ; Initialise the ARM interface, as follows: ; ; Port0(13..14} GPIO Output to on-board LEDs ; bit 13: USB_LINK LED ; bit 14: USB_CONNECT LED ; Port0{18,29} GPIO Input from BUT1 and BUT2 ; bit 18: BUT2 pressbutton input ; bit 29: BUT1 pressbutton input ; Port1{18,19} GPIO Inputs from joystick UP and DOWN ; bit 18: UP joystick inout ; bit 19: DOWN joystick input ;========================================================================= InitialiseARM_IO ; Define data direction for LEDs - the LEDs are connected ; to P0.13 and P0.14 STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR ;LEDs on ARM board: ; Assign GPIO on P0.13 and P0.14 LDR R5, =PINSEL0 ; Address of PINSEL0 LDR R0, [R5] ; Read current PINSEL0 BIC R0, R0, #0x3C000000 ; Clear bits 26..29 STR R0, [R5] ; Put back in PINSEL0 ; Configure output for P0.13 and P0.14 LDR R5, =IO0DIR ; Address of IO0DIR LDR R0, [R5] ; Read current IO0DIR ORR R0, R0, #0x6000 ; Set bits 13 and 14 - for P0.13 and P0.14 STR R0, [R5] ; Put back in IO0DIR ; Switches and Joystick inputs on ARM board ; Assign GPIO on P0.18 and P0.29 LDR R5, =PINSEL1 ; Address of PINSEL1 LDR R0, [R5] ; Read current PINSEL1 BIC R0, R0, #0x30 ; Clear bits 4..5 BIC R0, R0, #0x0c000000 ; Clear bits 26..27 STR R0, [R5] ; Put back in PINSEL0 ; Configure input for P0.18 and P0.29 LDR R5, =IO0DIR ; Address of IO0DIR LDR R0, [R5] ; Read current IO0DIR BIC R0, R0, #0x20000000 ; Clear bits 18 and 29 - for P0.18 and P0.29 BIC R0, R0, #0x00040000 STR R0, [R5] ; Put back in IO0DIR ; Assign GPIO on P1.18 and P1.19 LDR R5, =PINSEL3 ; Address of PINSEL3 LDR R0, [R5] ; Read current PINSEL3 BIC R0, R0, #0xF0 ; Clear bits 4..7 STR R0, [R5] ; Put back in PINSEL3 ; Configure input for P1.18 and P1.19 LDR R5, =IO1DIR ; Address of IO1DIR LDR R0, [R5] ; Read current IO1DIR BIC R0, R0, #0xc0000 ; Clear bits 18 and 19 - for P1.18 and P1.19 STR R0, [R5] ; Put back in IO1DIR LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;========================================================================= ; InitialiseApplicationsBoard_IO ; ; Initialise the interface to the Applications Board, as follows: ; ; Port4{0..7} GPIO Output to LEDs, Motor and Heater ; bits 0..4: Temperature error LED indicators ; bit 5: Heater control ; bits 6..7: Motor control ; Port4(8..15} GPIO Input from ADC ; bits 8..15: Inputs from Application Board ADC ; ;========================================================================= InitialiseApplicationsBoard_IO ; Define data direction for LEDs - the LEDs are connected ; to P0.13 and P0.14 STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR ; Applications Board Interface via Port4 ; Set FIO4MASK to allow access to all bits LDR R5, =FIO4MASK ; Address of FIO4DIR LDR R0, [R5] ; Read current IO4DIR MOV R0, #0x0 ; Clear all bits of FIO4MASK STR R0, [R5] ; Put back in IO4DIR ; Configure output for P4.0 to P4.7 LDR R5, =FIO4DIR ; Address of FIO4DIR LDR R0, [R5] ; Read current IO4DIR ORR R0, R0, #0xFF ; Set bits 0..7 - for P4.0 and P4.7 STR R0, [R5] ; Put back in IO4DIR ; Configure input for P4.8 to P4.15 LDR R5, =FIO4DIR ; Address of FIO4DIR LDR R0, [R5] ; Read current IO4DIR BIC R0, R0, #0xFF00 ; Clear bits 8..15 - for P4.8 and P4.15 STR R0, [R5] ; Put back in IO4DIR LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;========================================================================= ; Wait for BUT1 to be pressed ;========================================================================= WaitBUT1 STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR WaitForBUT1Pressed ldr r0, = IO0PIN ; Address of FIO0PIN register ldr r1, [r0] ; Read FIO0PIN in to r1 ands r1, r1, # B1_MASK ; Mask out BUT1 beq BUT1Pressed ; Exit LED toggle loop if button is pressed B WaitForBUT1Pressed BUT1Pressed LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;========================================================================= ; Wait for BUT2 to be pressed ;========================================================================= WaitBUT2 STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR WaitForBUT2Pressed ldr r0, = IO0PIN ; Address of FIO0PIN register ldr r1, [r0] ; Read FIO0PIN in to r1 ands r1, r1, # B2_MASK ; Mask out BUT1 beq BUT2Pressed ; Exit LED toggle loop if button is pressed B WaitForBUT2Pressed BUT2Pressed LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;========================================================================= ; Turn Fan Motor ON ;========================================================================= FanOn STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR LDR R5, =FIO4PIN ; Address of FIO4PIN LDR r0, [r5] ; Read current Port4 ORR r0, r0, #0x80 STR r0, [r5] ; Output LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;========================================================================= ; Turn Fan Motor OFF ;========================================================================= FanOff STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR LDR R5, =FIO4PIN ; Address of FIO4PIN LDR r0, [r5] ; Read current Port4 BIC r0, r0, #0xc0 STR r0, [r5] ; Output LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;============================================================================== ; Turn On Error LED ;============================================================================== errorLedOn STMFD r13!,{r0, r5,r14} mov r0, # USB_LINK_LED_MASK ldr r5, = IO0CLR str r0, [r5] LDMFD r13!,{r0, r5, r14} mov pc, r14 ;============================================================================== ; Turn Off Error LED ;============================================================================== errorLedOff STMFD r13!,{r0, r5,r14} mov r0, # USB_LINK_LED_MASK ldr r5, = IO0SET str r0, [r5] LDMFD r13!,{r0, r5, r14} mov pc, r14 ;============================================================================== ; Turn On System LED ;============================================================================== systemLedOn STMFD r13!,{r0, r5,r14} mov r0, # USB_CONNECT_LED_MASK ldr r5, = IO0CLR str r0, [r5] LDMFD r13!,{r0, r5, r14} mov pc, r14 ;============================================================================== ; Turn Off System LED ;============================================================================== systemLedOff STMFD r13!,{r0, r5,r14} mov r0, # USB_CONNECT_LED_MASK ldr r5, = IO0SET str r0, [r5] LDMFD r13!,{r0, r5, r14} mov pc, r14 ;============================================================================== ; Turn Heater On ;============================================================================== heaterOn STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR LDR R5, =FIO4PIN ; Address of FIO4PIN LDR r0, [r5] ; Read current Port4 ORR r0, r0, #0x20 STR r0, [r5] ; Output LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;============================================================================== ; Turn The Heater Off ;============================================================================== heaterOff STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR LDR R5, =FIO4PIN ; Address of FIO4PIN LDR r0, [r5] ; Read current Port4 AND r0, r0, #0xDF STR r0, [r5] ; Output LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;============================================================================== ; Read Temperature ;============================================================================== readTemp STMFD r13!,{r0,r5,r14} ; Push r0, r5 and LR LDR r0, =FIO4PIN LDR r1, [r0] LSR r1, r1, #8 AND r1, r1, #0xFF ; r1 now holds the temperature value LDMFD r13!,{r0,r5,r14} ; Pop r0, r5 and LR mov pc, r14 ; Put link register back into PC ;============================================================================== ; Implement Counter And Read Temperature ;============================================================================== checkTemp STMFD r13!,{r0, r14} ; Push PC, r0, Lr LSR r1, r1, #8 LDR r0, [r2] ; Load delay value in r0. SUB r0, r0, #0x01 ; Minus 1 from delay. BEQ readTemp ; Branch if timer counted down. errorVal STMFD r13!,{r0, r14} ; Push PC, r0, Lr SUB r1, r3, r6 ; Subtract set point from temperature, into r6 mov pc, r14 ; Put link register back into PC END ```