New motor func

// Motor "0" means magnitude is applied to all motors
void setMotorPWM(uint8_t motor_number, uint32_t motor_magnitude){
    uint8_t motor_gpio_pins = 0;

    if (motor_magnitude > 100) {
        motor_gpio_pins = 1;
    }
    if (motor_magnitude != 0) {
        motor_magnitude = motor_magnitude + 50;
    }


    switch (motor_number) {
        case 0:
            TIM1->CCR1 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            TIM2->CCR2 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            TIM2->CCR3 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            HAL_GPIO_WritePin(MOTOR_4_GPIO_Port, MOTOR_4_Pin, motor_gpio_pins);
            HAL_GPIO_WritePin(MOTOR_5_GPIO_Port, MOTOR_5_Pin, motor_gpio_pins);
            break;
        case 1:
            TIM1->CCR1 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            break;
        case 2:
            TIM2->CCR2 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            break;
        case 3:
            TIM2->CCR3 = (uint16_t)(MAX_PWM_PULSE_COUNT * ((float)motor_magnitude / 255.0));
            break;
        case 4:
            HAL_GPIO_WritePin(MOTOR_4_GPIO_Port, MOTOR_4_Pin, motor_gpio_pins);
            break;
        case 5:
            HAL_GPIO_WritePin(MOTOR_5_GPIO_Port, MOTOR_5_Pin, motor_gpio_pins);
            break;
    }
}