PROS, auton selector with LLEMU

initialize.cpp

//
// Copyright (c) 2018 by CMASS Robotics team. All Rights Reserved.
//

#include "main.h"
#include <string>
#include <cstdarg>
#include <memory>

short _auto_selected_id = 1;

bool IsBlueSide = false;

bool IsDebug = false;

char* format(const char* f, ...){
  va_list args;
  va_start(args, f);
  #ifndef _MSC_VER
	 	// Extra space for '\0'
    size_t size = std::snprintf( nullptr, 0, f, args) + 1;
    std::unique_ptr<char[]> buf( new char[ size ] );
    std::vsnprintf( buf.get(), size, f, args);
    return buf.get(); // We don't want the '\0' inside
  #else
    int size = _vscprintf(f, args);
    std::string result(++size, 0);
    return (char*)result.data()
  #endif
  va_end(args);
}

template <typename... Params>
void log(std::int16_t line, const char* fmt, Params... args) {
	pros::c::lcd_print(line, fmt, pros::lcd::convert_args(args)...);
}

void clearLog(){
  for(int i = 0; i < 8; i ++){
    pros::lcd::clear_line(i);
  }
}

uint8_t waitScreenBtnPress(){
  uint8_t old = pros::lcd::read_buttons();
  while(old == pros::lcd::read_buttons()){
    pros::delay(10);
  }
  return pros::lcd::read_buttons();
}

uint8_t isLeftBtnPress(){
  return pros::lcd::read_buttons() & LCD_BTN_LEFT;
}

uint8_t isCenterBtnPress(){
  return pros::lcd::read_buttons() & LCD_BTN_CENTER;
}

uint8_t isRightBtnPress(){
  return pros::lcd::read_buttons() & LCD_BTN_RIGHT;
}

std::string _get_program_name(short id){
 switch(id){
   case 0:
     return "Do nothing";
   case 1:
     return "AutoPlan1";
   case 2:
     return "AutoPlan2";
   case 3:
     return "AutoPlan3";
   case 4:
     return "AutoPlan4";
   case 5:
     return "AutoPlan5";
   case 6:
     return "AutoPlan6";
 }
 return "error";
}

std::string _get_side_name(bool isbs){
 return isbs ? "BLUE" : "RED";
}

void _select_program(){
	short min = 0; // 0 do nothing
	short max = 6; //so the list is 0-6
	while(true){
		if(isLeftBtnPress()){
			_auto_selected_id --;
		}else if(isRightBtnPress()){
			_auto_selected_id ++;
		}else if(isCenterBtnPress()){
			break;
		}
		if(_auto_selected_id < min){
			_auto_selected_id = max;
		}else if(_auto_selected_id > max){
			_auto_selected_id = min;
		}

		log(2, "Program: %s <<", _get_program_name(_auto_selected_id));
		log(3, "Side:     %s", _get_side_name(IsBlueSide));
    log(4, "Debug:    %d", IsDebug);
		waitScreenBtnPress();
	}
	waitScreenBtnPress();
}

void _select_side(){
	while(true){
		if(isLeftBtnPress() || isRightBtnPress()){
			IsBlueSide = !IsBlueSide;
		}else if(isCenterBtnPress()){
			break;
		}

		log(2, "Program: %s", _get_program_name(_auto_selected_id));
		log(3, "Side:     %s <<", _get_side_name(IsBlueSide));
    log(4, "Debug:    %d", IsDebug);
		waitScreenBtnPress();
	}
	waitScreenBtnPress();
}

void _select_debug(){
	while(true){
		if(isLeftBtnPress() || isRightBtnPress()){
			IsDebug = !IsDebug;
		}else if(isCenterBtnPress()){
			break;
		}

		log(2, "Program: %s", _get_program_name(_auto_selected_id));
		log(3, "Side:     %s", _get_side_name(IsBlueSide));
		log(4, "Debug:    %d <<", IsDebug);
		waitScreenBtnPress();
	}
	waitScreenBtnPress();
}

void disabled() {
	pros::lcd::print(0, "Auto programs selction");
	while(true){
		_select_program();
		_select_side();
		_select_debug();
	}
}