Fix STM sleep mode

[bradjc]

1. call PWR_EnterSTOPMode() instead
2. Fix main loop so interrupts can't occur before WFI
3. Look at stm-sleep branch for how to get the I2C to wake the STM
4. Figure out why just changing the sleep function doesn't work.