Working

Drivers/CMSIS/Device/ST/STM32L4xx/Include/stm32l432xx.h

@@ -45,8 +45,8 @@
   * @brief Configuration of the Cortex-M4 Processor and Core Peripherals
    */
 #define __CM4_REV                 0x0001U  /*!< Cortex-M4 revision r0p1                       */
-#define __MPU_PRESENT             1U       /*!< STM32L4XX provides an MPU                     */
-#define __NVIC_PRIO_BITS          4U       /*!< STM32L4XX uses 4 Bits for the Priority Levels */
+#define __MPU_PRESENT             1U       /*!< STM32L4xx provides an MPU                     */
+#define __NVIC_PRIO_BITS          4U       /*!< STM32L4xx uses 4 Bits for the Priority Levels */
 #define __Vendor_SysTickConfig    0U       /*!< Set to 1 if different SysTick Config is used  */
 #define __FPU_PRESENT             1U       /*!< FPU present                                   */
 
@@ -59,7 +59,7 @@
   */
 
 /**
- * @brief STM32L4XX Interrupt Number Definition, according to the selected device
+ * @brief STM32L4xx Interrupt Number Definition, according to the selected device
  *        in @ref Library_configuration_section
  */
 typedef enum

SHAL/Include/Peripheral/ADC/Reg/SHAL_ADC_REG_L432KC.h

@@ -16,7 +16,7 @@
 #define NUM_ADC_CHANNELS 16
 
 enum class SHAL_ADC_Channel : uint32_t {
-    CH0,
+    CH0 = 0,
     CH1,
     CH2,
     CH3,
@@ -83,14 +83,14 @@ static inline SHAL_ADC_Control_Reg getADCControlReg(ADC_Key key) {
 
 static inline SHAL_ADC_Config_Reg getADCConfigReg(ADC_Key key) {
 
-    SHAL_ADC_Config_Reg res = {nullptr, ADC_CFGR_CONT, ADC_CFGR_RES_Pos, ADC_CFGR_ALIGN_Pos};
+    SHAL_ADC_Config_Reg res = {nullptr, ADC_CFGR_CONT, ADC_CFGR_RES_Pos, ADC_CFGR_ALIGN_Pos, ADC_CFGR_CONT_Msk};
 
     res.reg = &(ADC_TABLE[static_cast<uint8_t>(key)]->CFGR);
     return res;
 }
 
 static inline SHAL_ADC_ISR_Reg getADCISRReg(ADC_Key key){
-    SHAL_ADC_ISR_Reg res = {nullptr, ADC_ISR_EOC, ADC_ISR_EOS, ADC_ISR_ADRDY};
+    SHAL_ADC_ISR_Reg res = {nullptr, ADC_ISR_EOC, ADC_ISR_EOS, ADC_ISR_ADRDY, ADC_ISR_OVR};
 
     res.reg = &(ADC_TABLE[static_cast<uint8_t>(key)]->ISR);
     return res;

SHAL/Include/Peripheral/ADC/SHAL_ADC_TYPES.h

@@ -39,6 +39,7 @@ struct SHAL_ADC_Config_Reg {
 
     uint32_t resolution_offset;
     uint32_t alignment_offset;
+    uint32_t continuous_mode_mask;
 };
 
 //Register for all ADC data
@@ -53,6 +54,7 @@ struct SHAL_ADC_ISR_Reg {
     uint32_t end_of_conversion_mask;
     uint32_t end_of_sequence_mask;
     uint32_t ready_mask;
+    uint32_t overrun_mask;
 };
 
 //Register controlling the clock source for the ADC

SHAL/Include/Peripheral/EXT/Reg/SHAL_EXTI_REG_L432KC.h

@@ -10,6 +10,15 @@
 
 #define EXTI_PENDING_REG(line) ((line) < 32 ? EXTI->PR1 : EXTI->PR2)
 
+static inline SHAL_EXTI_Control_Register getEXTIControlRegister(uint32_t line){
+    uint8_t maskOffset = line % 4; //Each register has four 4-bit wide fields
+    uint8_t registerOffset = line / 4; //Composed of four registers with 4 fields each
+
+    SHAL_EXTI_Control_Register res = {&SYSCFG->EXTICR[registerOffset], maskOffset};
+
+    return res;
+}
+
 static inline SHAL_EXTI_Interrupt_Mask_Register getEXTIInterruptMaskRegister(uint32_t line){
     volatile uint32_t* reg =  line < 32 ? &EXTI->IMR1 : &EXTI->IMR2;
     return {reg};;

SHAL/Include/Peripheral/EXT/SHAL_EXTI_TYPES.h

@@ -19,4 +19,9 @@ struct SHAL_EXTI_Falling_Trigger_Selection_Register {
     volatile uint32_t* reg;
 };
 
+struct SHAL_EXTI_Control_Register {
+    volatile uint32_t* reg;
+    uint32_t offset;
+};
+
 #endif //SHMINGO_HAL_SHAL_EXTI_TYPES_H

SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_REG_L432KC.h

@@ -17,15 +17,15 @@
 
 //Build enum map of available SHAL_GPIO pins
 enum class GPIO_Key : uint8_t {
-    A0,
-    A1,
-    A2,
-    A3,
-    A4,
-    A5,
-    A6,
-    A7,
-    A8,
+    A0 = 0,
+    A1 = 1,
+    A2 = 2,
+    A3 = 3,
+    A4 = 4,
+    A5 = 5,
+    A6 = 6,
+    A7 = 7,
+    A8 = 8,
     A9,
     A10,
     A11,
@@ -71,6 +71,10 @@ constexpr uint8_t getGPIOPinNumber(GPIO_Key key){
     return static_cast<uint8_t>(key) % 16;
 }
 
+constexpr uint8_t getGPIOPortNUmber(GPIO_Key key){
+    return static_cast<uint8_t>(key) / 16;
+}
+
 constexpr SHAL_GPIO_EXTI_Register getGPIOEXTICR(const GPIO_Key g){
     switch(g) {
         case GPIO_Key::A0: return {&SYSCFG->EXTICR[0],SYSCFG_EXTICR1_EXTI0_PA,EXTI0_IRQn};
@@ -120,7 +124,7 @@ constexpr uint32_t getGPIOPortNumber(const GPIO_Key g){
 
 static inline SHAL_GPIO_Mode_Register getGPIOModeRegister(const GPIO_Key key){
     volatile uint32_t* reg = &GPIO_TABLE[static_cast<uint8_t>(key) / 16]->MODER;
-    uint32_t offset = 2 * static_cast<uint8_t>(key) % 16;
+    uint32_t offset = 2 * (static_cast<uint8_t>(key) % 16);
     return {reg,offset};
 }
 
@@ -154,6 +158,12 @@ static inline SHAL_GPIO_Output_Type_Register getGPIOOutputTypeRegister(const GPI
 
 static inline SHAL_GPIO_Output_Data_Register getGPIOOutputDataRegister(const GPIO_Key key){
     volatile uint32_t* reg = &GPIO_TABLE[static_cast<uint8_t>(key) / 16]->ODR;
+    uint32_t offset = (static_cast<uint8_t>(key) % 16);
+    return {reg,offset};
+}
+
+static inline SHAL_GPIO_Input_Data_Register getGPIOInputDataRegister(const GPIO_Key key){
+    volatile uint32_t* reg = &GPIO_TABLE[static_cast<uint8_t>(key) / 16]->IDR;
     uint32_t offset = static_cast<uint8_t>(key) % 16;
     return {reg,offset};
 }

SHAL/Include/Peripheral/GPIO/SHAL_GPIO.h

@@ -29,6 +29,7 @@ public:
     /// \return ADC result
     uint16_t analogRead(SHAL_ADC_SampleTime sampleTime = SHAL_ADC_SampleTime::C8);
 
+    uint16_t digitalRead();
 
     void setAlternateFunction(GPIO_Alternate_Function AF) volatile;
     void setAlternateFunction(GPIO_Alternate_Function_Mapping AF) volatile;

SHAL/Include/Peripheral/GPIO/SHAL_GPIO_TYPES.h

@@ -50,6 +50,11 @@ struct SHAL_GPIO_Output_Data_Register {
     uint32_t offset;
 };
 
+struct SHAL_GPIO_Input_Data_Register {
+    volatile uint32_t* reg;
+    uint32_t offset;
+};
+
 struct SHAL_GPIO_Port_Info{
     uint8_t number;
     SHAL_ADC_Channel ADCChannel;

SHAL/Include/Peripheral/Timer/Reg/SHAL_TIM_REG_L432KC.h

@@ -178,7 +178,7 @@ getTimerCaptureCompareModeRegistersOutput(Timer_Key key) {
 
 static inline SHAL_TIM_Break_Dead_Time_Register getTimerBreakDeadTimeRegister(Timer_Key key){
 
-    SHAL_TIM_Break_Dead_Time_Register res = {nullptr, 1UL << 15};
+    SHAL_TIM_Break_Dead_Time_Register res = {nullptr, TIM_BDTR_MOE};
 
     volatile TIM_TypeDef* tim = TIM_TABLE[static_cast<uint8_t>(key)];
 

SHAL/Include/Peripheral/Timer/SHAL_TIM.h

@@ -21,7 +21,7 @@ public:
     /// Initializes a timer
     /// \param prescaler The amount of times the base clock has to cycle before the timer adds one to the count
     /// \param autoReload The number of timer counts before the count is reset and IRQ is called
-    void init(uint32_t prescaler, uint32_t autoReload);
+    void init(uint16_t prescaler, uint16_t autoReload);
 
     //Starts the counter
     void start();

SHAL/Src/STM32L4xx/Peripheral/ADC/SHAL_ADC.cpp

@@ -72,29 +72,44 @@ SHAL_Result SHAL_ADC::calibrate() {
 }
 
 uint16_t SHAL_ADC::singleConvertSingle(SHAL_ADC_Channel channel, SHAL_ADC_SampleTime time) {
+    auto data_reg = getADCDataReg(m_ADCKey);
+    auto ISR_reg = getADCISRReg(m_ADCKey);
+    auto config_reg = getADCConfigReg(m_ADCKey);
 
-    auto data_reg = getADCDataReg(m_ADCKey); //Where our output will be stored
+    SHAL_clear_bitmask(config_reg.reg, config_reg.continuous_mode_mask);
 
-    auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey,channel);
+    auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey, channel);
+    SHAL_set_bits(sampleTimeReg.reg, 3, static_cast<uint8_t>(time), sampleTimeReg.channel_offset);
 
-    SHAL_set_bits(sampleTimeReg.reg,3,static_cast<uint8_t>(time),sampleTimeReg.channel_offset); //Set sample time register TODO un-hardcode bit width?
+    addADCChannelToSequence(channel, 0);
+    if(setADCSequenceAmount(1) == SHAL_Result::ERROR) { return 0; }
 
-    addADCChannelToSequence(channel,0); //Use index 0 to convert channel
-    if(setADCSequenceAmount(1) == SHAL_Result::ERROR){return 0;} //Since we're using single convert, convert 1 channel
-
-    if(enable() != SHAL_Result::OKAY){
+    if(enable() != SHAL_Result::OKAY) {
         return 0;
     }
 
-    startConversion(); //Start ADC conversion
-
-    auto ISR_reg = getADCISRReg(m_ADCKey);
-
-    if(!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_sequence_mask) != 0),500)){ //Wait for conversion
-        return 0; //Failed
+    // CRITICAL: Clear ALL relevant flags before starting
+    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.end_of_sequence_mask);
+    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.end_of_conversion_mask);
+    if(ISR_reg.overrun_mask) {
+        SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.overrun_mask);
     }
 
-    return *data_reg.reg;
+    volatile uint16_t dummy = *data_reg.reg;
+    (void)dummy;
+
+    startConversion();
+
+    if(!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_conversion_mask) != 0), 2000)) {
+        return 0;
+    }
+
+    uint16_t result = *data_reg.reg;
+
+    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.end_of_conversion_mask);
+    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.end_of_sequence_mask);
+
+    return result;
 }
 
 SHAL_Result SHAL_ADC::multiConvertSingle(SHAL_ADC_Channel* channels, const int numChannels, uint16_t* result, SHAL_ADC_SampleTime time) {
@@ -259,27 +274,23 @@ SHAL_Result SHAL_ADC::setADCSequenceAmount(uint32_t amount) {
 }
 
 SHAL_Result SHAL_ADC::addADCChannelToSequence(SHAL_ADC_Channel channel, uint32_t index) {
-
-    if(!isValid()){return SHAL_Result::ERROR;}
+    if(!isValid()) { return SHAL_Result::ERROR; }
 
     auto sequenceRegisters = getADCSequenceRegisters(m_ADCKey);
-
     auto channelNum = static_cast<uint8_t>(channel);
 
-    uint32_t bitSection = (index + 1) % 5; //Need a new variable since SQR1 has its data bits shifted up by one section to make room for the L section
+    uint32_t bitSection = (index + 1) % 5;
     uint32_t sequenceRegNumber = (index + 1) / 5;
 
     volatile uint32_t* sequenceReg = sequenceRegisters.regs[sequenceRegNumber];
     uint32_t bitSectionOffset = sequenceRegisters.offsets[bitSection];
 
-    if(sequenceRegNumber != 0){
-        *sequenceReg = 0; //Clear previous conversions
-    }
-    else{
-        *sequenceReg &= 0x0000000F;
-    }
+    // Clear only the specific 5 bits we're setting, not the entire register
+    uint32_t clearMask = ~(0x1F << bitSectionOffset);
+    *sequenceReg &= clearMask;
 
-    SHAL_set_bits(sequenceReg,5,channelNum,bitSectionOffset);
+    // Set the new channel number
+    *sequenceReg |= (channelNum << bitSectionOffset);
 
     return SHAL_Result::OKAY;
 }

SHAL/Src/STM32L4xx/Peripheral/GPIO/SHAL_GPIO.cpp

@@ -64,7 +64,6 @@ SHAL_Result SHAL_GPIO::setPinMode(PinMode mode) volatile {
         SHAL_UART2.sendString(buff);
         return SHAL_Result::ERROR;
     }
-    SHAL_print_register(pinModeReg.reg);
 
     SHAL_set_bits(pinModeReg.reg,2,static_cast<uint8_t>(mode),pinModeReg.offset); //Set mode
 
@@ -73,31 +72,20 @@ SHAL_Result SHAL_GPIO::setPinMode(PinMode mode) volatile {
 
 void SHAL_GPIO::useAsExternalInterrupt(TriggerMode mode, EXTICallback callback) {
 
-    uint32_t gpioPin = getGPIOPinNumber(m_GPIO_KEY);
 
-    setPinMode(PinMode::INPUT_MODE); //Explicitly set mode to input
+    /* ---- Connect PB6 to EXTI6 via SYSCFG ---- */
+    uint32_t port_b_val = 1; // 0=A, 1=B, 2=C, 3=D, etc.
+    SYSCFG->EXTICR[1] &= ~(0xFUL << 8);     // Clear EXTI6 bits (bits 8-11)
+    SYSCFG->EXTICR[1] |=  (port_b_val << 8); // Set EXTI6 to PB6
 
-    RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; //Enable EXT, TODO Add this to a global SHAL_GLOBAL_TYPES.h file
-    NVIC_EnableIRQ(getGPIOEXTICR(m_GPIO_KEY).IRQN); //Enable IRQN for pin
+    /* ---- Configure EXTI line 6 ---- */
+    EXTI->IMR1  |=  (1UL << 6);  // Unmask line 6
+    EXTI->RTSR1 |=  (1UL << 6);  // Rising trigger enable
+    EXTI->FTSR1 &= ~(1UL << 6);  // Falling trigger disable
 
-    auto ext_imr = getEXTIInterruptMaskRegister(gpioPin);
-    SHAL_set_bits(ext_imr.reg,1,1,gpioPin);
-
-    SHAL_GPIO_EXTI_Register EXTILineEnable = getGPIOEXTICR(m_GPIO_KEY);
-    *EXTILineEnable.EXT_ICR |= EXTILineEnable.mask; //Set bits to enable correct port on correct line TODO Find way to clear bits before
-
-    if(mode == TriggerMode::RISING_EDGE || mode == TriggerMode::RISING_FALLING_EDGE) {
-        auto rising_trigger_selection_reg = getEXTIRisingTriggerSelectionRegister(gpioPin);
-        SHAL_set_bits(rising_trigger_selection_reg.reg, 1, 1, gpioPin);
-    }
-
-    if(mode == TriggerMode::FALLING_EDGE || mode == TriggerMode::RISING_FALLING_EDGE) {
-        auto falling_trigger_selection_reg = getEXTIFallingTriggerSelectionRegister(gpioPin);
-        SHAL_set_bits(falling_trigger_selection_reg.reg,1,1,gpioPin);
-    }
-
-    //Set callback
-    registerEXTICallback(m_GPIO_KEY,callback);
+    /* ---- Enable NVIC interrupt for EXTI lines [9:5] ---- */
+    NVIC_SetPriority(EXTI9_5_IRQn, 2);
+    NVIC_EnableIRQ(EXTI9_5_IRQn);
 
     __enable_irq(); //Enable IRQ just in case
 }
@@ -115,6 +103,15 @@ void SHAL_GPIO::setAlternateFunction(GPIO_Alternate_Function_Mapping AF) volatil
     SHAL_set_bits(alternateFunctionReg.reg,4,static_cast<uint8_t>(AF),alternateFunctionReg.offset);
 }
 
+uint16_t SHAL_GPIO::digitalRead() {
+    auto inputDataReg = getGPIOInputDataRegister(m_GPIO_KEY);
+
+    if((*inputDataReg.reg & (1 << 6)) != 0){
+        return 1;
+    }
+    return 0;
+}
+
 
 SHAL_GPIO& GPIOManager::get(GPIO_Key key) {
 

SHAL/Src/STM32L4xx/Peripheral/Timer/SHAL_TIM.cpp

@@ -17,11 +17,14 @@ void Timer::start() {
 
     auto control_reg = getTimerControlRegister1(m_key);
     auto event_generation_reg = getTimerEventGenerationRegister(m_key);
+    auto status_reg = getTimerStatusRegister(m_key);
 
     SHAL_apply_bitmask(control_reg.reg, control_reg.counter_enable_mask); //Enable counter
     SHAL_apply_bitmask(control_reg.reg, control_reg.auto_reload_preload_enable_mask); //Preload enable (buffer)
     SHAL_apply_bitmask(event_generation_reg.reg, event_generation_reg.update_generation_mask);
 
+    SHAL_clear_bitmask(status_reg.reg,status_reg.update_interrupt_flag_mask);
+
     enableInterrupt();
 }
 
@@ -47,12 +50,15 @@ void Timer::enableInterrupt() {
     NVIC_EnableIRQ(getTimerIRQn(m_key)); //Enable the IRQn in the NVIC
 }
 
-void Timer::init(uint32_t prescaler, uint32_t autoReload) {
+void Timer::init(uint16_t prescaler, uint16_t autoReload) {
     SHAL_TIM_RCC_Register rcc = getTimerRCC(m_key);
     SHAL_apply_bitmask(rcc.reg,rcc.enable_mask);
 
     setPrescaler(prescaler);
     setARR(autoReload);
+
+    *getTimerStatusRegister(m_key).reg = 0;
+    *getTimerDMAInterruptEnableRegister(m_key).reg = 0;
 }
 
 void Timer::setPWMMode(SHAL_Timer_Channel channel, SHAL_TIM_Output_Compare_Mode outputCompareMode, SHAL_Timer_Channel_Main_Output_Mode mainOutputMode,
@@ -86,6 +92,7 @@ void Timer::setPWMMode(SHAL_Timer_Channel channel, SHAL_TIM_Output_Compare_Mode
 
     SHAL_set_bits(ccer.reg, 4, fullChannelModeMask, offset);
     SHAL_apply_bitmask(bdtr.reg, bdtr.main_output_enable_mask);
+
 }
 
 void Timer::setPWMDutyCycle(uint32_t dutyCycle) {

SHAL/Src/STM32L4xx/Peripheral/Timer/SHAL_TIM_CALLBACK.cpp

@@ -8,8 +8,8 @@ DEFINE_TIMER_IRQ(Timer_Key::S_TIM1, TIM1_IRQHandler)
 DEFINE_TIMER_IRQ(Timer_Key::S_TIM2, TIM2_IRQHandler)
 DEFINE_TIMER_IRQ(Timer_Key::S_TIM6, TIM6_IRQHandler)
 DEFINE_TIMER_IRQ(Timer_Key::S_TIM7, TIM7_IRQHandler)
-DEFINE_TIMER_IRQ(Timer_Key::S_TIM15, TIM15_IRQHandler)
-DEFINE_TIMER_IRQ(Timer_Key::S_TIM16, TIM16_IRQHandler)
+DEFINE_TIMER_IRQ(Timer_Key::S_TIM15, TIM1_BRK_TIM15_IRQHandler)
+DEFINE_TIMER_IRQ(Timer_Key::S_TIM16, TIM1_UP_TIM16_IRQHandler)
 
 void registerTimerCallback(Timer_Key key, TimerCallback callback){
     timer_callbacks[static_cast<uint32_t>(key)] = callback;

SHAL/Src/main.cpp

@@ -1,40 +1,147 @@
 #include <cstdio>
 #include "SHAL.h"
 
-GPIO_Key gpios[6] = {
-        GPIO_Key::A0,
-        GPIO_Key::A1,
-        GPIO_Key::A4,
-        GPIO_Key::A5,
-        GPIO_Key::A6,
-        GPIO_Key::A7,
+
+#define NUM_CHANNELS 6
+
+// Physical order on right-side header: A0, A1, A3, A4, A5, A6, A7
+SHAL_ADC_Channel channels[NUM_CHANNELS] = {
+        SHAL_ADC_Channel::CH5,
+        SHAL_ADC_Channel::CH6,
+        SHAL_ADC_Channel::CH8,
+        SHAL_ADC_Channel::CH9,
+        SHAL_ADC_Channel::CH10,
+        SHAL_ADC_Channel::CH12,
 };
 
+bool isDeviceOn = false;
+bool shouldToggleDeviceState = true;
+bool shouldCheckSensorThresholds = true;
+
+uint16_t vals[NUM_CHANNELS] = {0,0,0,0,0,0};
+uint8_t currentSensor = 0;
+
 bool isAlarmBeeping = false;
-bool isCalibrateButtonHigh = false;
 
-uint16_t sensorThresholds[6] = {4096,4096,4096,4096,4096,4096};
+uint16_t sensorThresholds[NUM_CHANNELS] = {0,0,0,0,0,0};
 
-void timer2callback(){
+int buzzer_beepCount = 0;
+bool isBeepingForCalibration = false;
 
-    uint16_t val[6];
+bool prevIsCalibrateButtonHigh = false;
 
-    for(int i = 0; i < 6; i++){
-        val[i] = GPIOManager::get(gpios[i]).analogRead(SHAL_ADC_SampleTime::C8);
-        SHAL_delay_ms(30);
+int cyclesPerPrint = 2;
+int currentCycle = 0;
+
+bool areSensorRequirementsMetCurrent = false;
+bool areSensorRequirementsMetPrevious = false;
+
+void getSensorData(){
+
+    vals[currentSensor] = SHAL_ADC1.singleConvertSingle(channels[currentSensor]);
+
+    if(currentSensor == (NUM_CHANNELS - 1) && currentCycle == cyclesPerPrint - 1){
+        char buff[125];
+        // Print in the same order as the channels[] array (physical order)
+        sprintf(buff, "A0:%u,A1:%u,A3:%u,A4:%u,A5:%u,A6:%u\r\n",
+                vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
+        SHAL_UART2.sendString(buff);
     }
 
-    char buff[64];
-    sprintf(buff, "%d, %d, %d, %d, %d, %d\r\n", val[0],val[1],val[2],val[3],val[4],val[5]);
+    currentSensor = (currentSensor + 1) % NUM_CHANNELS;
+    currentCycle = (currentCycle + 1) % cyclesPerPrint;
+}
 
+void startBeeping(){
+
+    SHAL_TIM6.setPrescaler(4000);
+    SHAL_TIM6.setARR(200);
+
+    SHAL_TIM6.start();
+}
+
+void stopBeeping(){
+    SHAL_TIM1.stop();
+    SHAL_TIM6.stop();
+    isAlarmBeeping = false;
+    isBeepingForCalibration = false;
+}
+
+void checkSensorThresholds(){
+
+    bool localFlag = true;
+
+    for(int i = 0; i < NUM_CHANNELS; i++){
+        if(vals[i] < sensorThresholds[i]){
+            areSensorRequirementsMetCurrent = false; //Conditions not met
+            localFlag = false;
+            break;
+        }
+    }
+    if(localFlag){
+        areSensorRequirementsMetCurrent = true;
+    }
+
+    if(areSensorRequirementsMetCurrent){
+        if(!areSensorRequirementsMetPrevious){
+            SHAL_TIM1.stop();
+            SHAL_TIM6.stop();
+            SHAL_TIM15.stop();
+            PIN(A9).setLow();
+            stopBeeping();
+        }
+    }
+    else{
+        if(areSensorRequirementsMetPrevious){
+            SHAL_TIM15.start();
+            PIN(A9).setHigh();
+        }
+    }
+    areSensorRequirementsMetPrevious = areSensorRequirementsMetCurrent;
+}
+
+void calibrateThresholds(){
+
+    // Read every channel once and set threshold to 80% of reading
+    for(int i = 0; i < NUM_CHANNELS; i++){
+        uint16_t sensorVal = (vals[i] * 3) / 5;
+
+        if(sensorVal < 50){
+            sensorVal = 0;
+        }
+        else{
+            sensorVal = sensorVal - 50;
+        }
+        sensorThresholds[i] = sensorVal;
+    }
+
+    char buff[125];
+    // Print in the same order as the channels[] array (physical order)
+    sprintf(buff, "Thresholds calibrated to: A0:%u,A1:%u,A3:%u,A4:%u,A5:%u,A6:%u\r\n",
+            sensorThresholds[0], sensorThresholds[1], sensorThresholds[2], sensorThresholds[3], sensorThresholds[4], sensorThresholds[5]);
     SHAL_UART2.sendString(buff);
-
 }
 
 void PWMToggle(){
 
+    //Flash light
+    PIN(A9).toggle();
+
+    SHAL_TIM15.stop(); //Stop timer for allowed time off sensors
+
+    if(isBeepingForCalibration && buzzer_beepCount > 2){
+        isBeepingForCalibration = false;
+        buzzer_beepCount = 0;
+        SHAL_TIM6.stop(); //Reset timer 6
+        SHAL_TIM1.stop(); //Stop buzzer
+
+        SHAL_TIM6.setPrescaler(4000);
+        SHAL_TIM6.setARR(400);
+    }
+
     if(!isAlarmBeeping){
         SHAL_TIM1.start();
+        buzzer_beepCount++;
     }
     else{
         SHAL_TIM1.stop();
@@ -42,57 +149,116 @@ void PWMToggle(){
     isAlarmBeeping = !isAlarmBeeping;
 }
 
-void buttonCallback(){
+void buttonHoldCallback(){
 
-}
+    shouldCheckSensorThresholds = false; //Dont check sensor thresholds yet, ensure that calibration beep happens
 
-void calibrateSensors(){
-    PIN(B3).setHigh();
-    SHAL_TIM7.stop();
+    SHAL_TIM7.stop(); //Stop this timer
+
+    SHAL_TIM2.stop(); //Stop reading from ADC
+
+    buzzer_beepCount = 0;
+    isBeepingForCalibration = true;
+
+    SHAL_TIM6.init(4000,50);
+    SHAL_TIM6.start();
+
+    calibrateThresholds();
+    SHAL_TIM1.start();
+
+    SHAL_TIM2.start(); //Restart value checks
+
+    shouldToggleDeviceState = false;
+    shouldCheckSensorThresholds = true;
 }
 
 int main() {
 
     SHAL_init();
 
-    SHAL_UART2.init(UART_Pair_Key::Tx2A2_Rx2A3);
-    SHAL_UART2.begin(115200);
+    //SHAL_UART2.init(UART_Pair_Key::Tx2A2_Rx2A3);
+    //SHAL_UART2.begin(115200);
 
     PIN(A0).setPinMode(PinMode::ANALOG_MODE);
     PIN(A1).setPinMode(PinMode::ANALOG_MODE);
+    PIN(A3).setPinMode(PinMode::ANALOG_MODE);
     PIN(A4).setPinMode(PinMode::ANALOG_MODE);
     PIN(A5).setPinMode(PinMode::ANALOG_MODE);
     PIN(A6).setPinMode(PinMode::ANALOG_MODE);
     PIN(A7).setPinMode(PinMode::ANALOG_MODE);
 
+    PIN(B6).setPinMode(PinMode::INPUT_MODE);
+    PIN(A9).setPinMode(PinMode::OUTPUT_MODE);
     PIN(B0).setAlternateFunction(GPIO_Alternate_Function_Mapping::B0_TIM1CH2N);
 
-    PIN(B6).setPinMode(PinMode::INPUT_MODE);
-    PIN(B6).useAsExternalInterrupt(TriggerMode::RISING_FALLING_EDGE,buttonCallback);
+    PIN(A8).setPinMode(PinMode::OUTPUT_MODE);
+    PIN(A8).setInternalResistor(InternalResistorType::NO_PULL);
 
-    SHAL_TIM2.init(4000000,400);
+    SHAL_TIM2.init(4000,200);
 
-    SHAL_TIM2.setCallbackFunc(timer2callback);
+    SHAL_TIM2.setCallbackFunc(getSensorData);
     SHAL_TIM2.enableInterrupt();
     SHAL_TIM2.start();
 
-    SHAL_TIM1.init(300,999);
-    SHAL_TIM1.setPWMMode(SHAL_Timer_Channel::CH2,SHAL_TIM_Output_Compare_Mode::PWMMode1,SHAL_Timer_Channel_Main_Output_Mode::Disabled,SHAL_Timer_Channel_Complimentary_Output_Mode::Polarity_Reversed);
-    SHAL_TIM1.setPWMDutyCycle(499);
-    SHAL_TIM1.start();
+    SHAL_TIM1.init(0,2400); //PWM signal
+    SHAL_TIM1.setPWMMode(SHAL_Timer_Channel::CH2,SHAL_TIM_Output_Compare_Mode::PWMMode1,SHAL_Timer_Channel_Main_Output_Mode::Polarity_Normal,SHAL_Timer_Channel_Complimentary_Output_Mode::Polarity_Reversed);
+    SHAL_TIM1.setPWMDutyCycle(900);
 
-    SHAL_TIM6.init(4000000,400);
+    SHAL_TIM6.init(4000,500); //PWM switcher
     SHAL_TIM6.setCallbackFunc(PWMToggle);
     SHAL_TIM6.enableInterrupt();
-    SHAL_TIM6.start();
 
-    SHAL_TIM7.init(4000000,3000);
-    SHAL_TIM7.setCallbackFunc(calibrateSensors);
+    SHAL_TIM7.init(4000,3000); //Calibrate timer
+    SHAL_TIM7.setCallbackFunc(buttonHoldCallback);
     SHAL_TIM7.enableInterrupt();
 
-    PIN(B3).setPinMode(PinMode::OUTPUT_MODE); //Test
+    SHAL_TIM15.init(4000,5000); //5 seconds
+    SHAL_TIM15.setCallbackFunc(startBeeping);
+    SHAL_TIM15.enableInterrupt();
 
-    while (true) {
-        SHAL_UART2.sendString("HELLO\r\n");
+
+    SHAL_UART2.sendString("Hello3\r\n");
+
+    while (true) { //TODO set to use button for simulating off sensor, uncomment for real functionality
+
+        if(PIN(B6).digitalRead() != 1){
+
+            if(prevIsCalibrateButtonHigh){
+                SHAL_TIM7.start();
+            }
+            prevIsCalibrateButtonHigh = false;
+
+        }
+        else{
+            if(!prevIsCalibrateButtonHigh){
+                if(shouldToggleDeviceState){
+                    if(!isDeviceOn){ //Turn device on
+                        PIN(A8).setHigh();
+                        isDeviceOn = true;
+                    }
+                    else{ //Turn device off
+                        PIN(A8).setLow();
+                        PIN(A9).setLow();
+                        isDeviceOn = false;
+
+                        areSensorRequirementsMetCurrent = true;
+                        areSensorRequirementsMetPrevious = true;
+
+                        SHAL_TIM15.stop();
+                        stopBeeping();
+                    }
+
+                }
+
+                shouldToggleDeviceState = true;
+
+                SHAL_TIM7.stop();
+            }
+            prevIsCalibrateButtonHigh = true;
+        }
+
+        if(isDeviceOn && shouldCheckSensorThresholds){
+            checkSensorThresholds();
+        }
     }
 }