Lego Technic of the Past Arduino Library

HEADER

/* LegoTechnic.h - Library to run Lego Technic Controller brick with a specific Arduino Shield. Created November 16, 2015 Rights Reserved

• /

#ifndef LegoTechnic_h
#define LegoTechnic_h

#include "Arduino.h"

class LegoTechnic {

 public:

#define PORT0_PIN 3
#define PORT1_PIN 5
#define PORT2_PIN 6
#define PORT3_PIN 9
#define PORT4_PIN 10
#define PORT5_PIN 11

#define PORT6_ANALOGPIN 0
#define PORT7_ANALOGPIN 1

#define PORT_ON HIGH
#define PORT_OFF LOW

#define COMBO_OFF 0
#define COMBO_LEFT 1
#define COMBO_RIGHT 2
#define COMBO_BOTH 3

   LegoTechnic();
   LegoTechnic(int P0, int P1, int P2, int P3, int P4, int P5, int P6, int P7);
   void Output_Port(int id, int action);
   void PWM_Port(int id, int pwm_value);
   int Input_Port(int id);
   void Combo_Port(char id, int mix);
   void PWM_Combo_Port(char id, int mix, int pwm_value);
   void Port_Initialize();
   int Port_PWM[6] = {255, 255, 255, 255, 255, 255};
   void Verbose(int value);
   void Parse_Command(String Command);
 private:
   int _D0,_D1,_D2,_D3,_D4,_D5,_A6,_A7;
   int _Verbose = HIGH;
   void _Parse_PWM_Command(String Port, String Value);
   void _Parse_Verbose_Command(String Value);
   void _Parse_CPWM_Command(String Port, String Action, String Value);
   void _Parse_Port_Command(String Port, String Action);
   void _Parse_Combo_Command(String Port, String Action);
   void _Parse_Input_Command(String Port);
   void _Display_Commands();
   void _Display_PWM_Values();

};

#endif

CODE

#include "Arduino.h"
#include "LegoTechnic.h"

#define PORT_ON HIGH
#define PORT_OFF LOW

#define COMBO_OFF 0
#define COMBO_LEFT 1
#define COMBO_RIGHT 2
#define COMBO_BOTH 3

LegoTechnic::LegoTechnic() {

 _D0 = PORT0_PIN;
 _D1 = PORT1_PIN;
 _D2 = PORT2_PIN;
 _D3 = PORT3_PIN;
 _D4 = PORT4_PIN;
 _D5 = PORT5_PIN;
 _A6 = PORT6_ANALOGPIN;
 _A7 = PORT7_ANALOGPIN;

}

LegoTechnic::LegoTechnic(int P0, int P1, int P2, int P3, int P4, int P5, int P6, int P7) {

 _D0 = P0;
 _D1 = P1;
 _D2 = P2;
 _D3 = P3;
 _D4 = P4;
 _D5 = P5;
 _A6 = P6;
 _A7 = P7;

}

void LegoTechnic::Output_Port(int id, int action) {

 int pin_id;
 int pin_value;
 
 int valid_command = 1;
 
 switch (id)
 {
   case 0:
     pin_id = _D0;
   break;
   case 1:
     pin_id = _D1;
   break;
   case 2:
     pin_id = _D2;
   break;
   case 3:
     pin_id = _D3;
   break;
   case 4:
     pin_id = _D4;
   break;
   case 5:
     pin_id = _D5;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Port Identified.");}
     valid_command = 0;
   break;  
 }
 
 switch (action)
 {
   case PORT_ON:
     pin_value = HIGH;
   break;
   case PORT_OFF:
     pin_value = LOW;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Port Status Identified.");}
     valid_command = 0;
 }
 
 if (valid_command == 1)
   {
     digitalWrite(pin_id, pin_value);
     if (_Verbose == HIGH) {Serial.println("Success PORT Command");}
   }
   

}

void LegoTechnic::PWM_Port(int id, int pwm_value) {

 int pin_id;
 
 int valid_command =1;
 
 switch(id)
 {
   case 0:
     pin_id = _D0;
   break;
   case 1:
     pin_id = _D1;
   break;
   case 2:
     pin_id = _D2;
   break;
   case 3:
     pin_id = _D3;
   break;
   case 4:
     pin_id = _D4;
   break;
   case 5:
     pin_id = _D5;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid PWM Port Identified.");}
     valid_command = 0;
   break;
 }
 
 if ((pwm_value < 0) or (pwm_value > 255))
   {
     if (_Verbose == HIGH) {Serial.println("Invalid PWM Value Identified.");}
     valid_command = 0;
   }
   
 if (valid_command == 1)
   {
     Port_PWM[id] = pwm_value;
     analogWrite(pin_id, pwm_value);
     if (_Verbose == HIGH) {Serial.println("Success PWM Command.");}
   }
   

}

int LegoTechnic::Input_Port(int id) {

 int valid_command = 1;
 int pin_id;
 
 switch(id)
   {
     case 6:
       pin_id = _A6;
     break;
     case 7:
       pin_id = _A7;
     break;
     default:
       if (_Verbose == HIGH) {Serial.println("Invalid Input Port Identified.");}
       valid_command = 0;
     break;
   }
   
 if (valid_command == 1)
   { 
     if (_Verbose == HIGH) {Serial.println("Success Input Command.");}
     return analogRead(pin_id);
   }
  else
   return -1; 

}

void LegoTechnic::Combo_Port(char id, int mix) {

 int leftport, rightport;
 int leftvalue, rightvalue;
 int valid_command = 1;
 
 switch (id)
 {
   case 'A':
     leftport = 0;
     rightport = 1;
   break;
   case 'B':
     leftport = 2;
     rightport = 3;
   break;
   case 'C':
     leftport = 4;
     rightport = 5;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Combo Port Identified.");}
     valid_command = 0;
   break;
 }

 switch (mix)
 {
   case COMBO_OFF:
     leftvalue = LOW;
     rightvalue = LOW;
   break;
   case COMBO_LEFT:
     leftvalue = HIGH;
     rightvalue = LOW;
   break;
   case COMBO_RIGHT:
     leftvalue = LOW;
     rightvalue = HIGH;
   break;
   case COMBO_BOTH:
     leftvalue = HIGH;
     rightvalue = HIGH;
   break;  
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Combo Port Action.");}
     valid_command =0;
   break;
 }  
 
 if (valid_command ==1) 
 {
   Output_Port(leftport, leftvalue);
   Output_Port(rightport, rightvalue);
   if (_Verbose == HIGH) {Serial.println("Successful COMBO Command.");}
 }
 

}

void LegoTechnic::PWM_Combo_Port(char id, int mix, int pwm_value) {

 int leftport, rightport;
 int leftvalue, rightvalue;
 int valid_command = 1;
 
 switch (id)
 {
   case 'A':
     leftport = 0;
     rightport = 1;
   break;
   case 'B':
     leftport = 2;
     rightport = 3;
   break;
   case 'C':
     leftport = 4;
     rightport = 5;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Combo Port Identified.");}
     valid_command = 0;
   break;
 }

 switch (mix)
 {
   case COMBO_OFF:
     leftvalue = 0;
     rightvalue = 0;
   break;
   case COMBO_LEFT:
     leftvalue = pwm_value;
     rightvalue = 0;
   break;
   case COMBO_RIGHT:
     leftvalue = 0;
     rightvalue = pwm_value;
   break;
   case COMBO_BOTH:
     leftvalue = pwm_value;
     rightvalue = pwm_value;
   break;  
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Combo Port Action.");}
     valid_command = 0;
   break;
 }  
 
 if (valid_command ==1) 
 {
   PWM_Port(leftport, leftvalue);
   PWM_Port(rightport, rightvalue);
   if (_Verbose == HIGH) {Serial.println("Successful CPWM Command.");}
 }
 

}

void LegoTechnic::Port_Initialize() {

 pinMode(_D0, OUTPUT);
 analogWrite(_D0, 255);
 digitalWrite(_D0, LOW);

 pinMode(_D1, OUTPUT);
 analogWrite(_D1, 255);
 digitalWrite(_D1, LOW);

 pinMode(_D2, OUTPUT);
 analogWrite(_D2, 255);
 digitalWrite(_D2, LOW);

 pinMode(_D3, OUTPUT);
 analogWrite(_D3, 255);
 digitalWrite(_D3, LOW);

 pinMode(_D4, OUTPUT);
 analogWrite(_D4, 255);
 digitalWrite(_D4, LOW);

 pinMode(_D5, OUTPUT);
 analogWrite(_D5, 255);
 digitalWrite(_D5, LOW);

 if (_Verbose == HIGH) {Serial.println("Output Port PWM values set to 255.");}

}

void LegoTechnic::Verbose(int value) {

 _Verbose = value;

}

void LegoTechnic::Parse_Command(String Command) {

 int valid_command = 0;
 String sub_string;
 String command_string;
 String port_string;
 String action_string;
 String value_string;
 
 if (_Verbose == HIGH) {Serial.println(Command);}

 sub_string = Command.substring(0,4);
 
 if (sub_string.compareTo("?PWM") == 0)
   {
     _Display_PWM_Values();
     valid_command = 1;
   }
 
 sub_string = Command.substring(0,2);
 
 if (sub_string.compareTo("?") == 0)
   {
      _Display_Commands();
      valid_command = 1;
   }
 
 sub_string = Command.substring(0,4);
 
 if (sub_string.compareTo("PORT") == 0 )
   {
     command_string = "PORT";
     port_string = Command.substring(5,6);
     action_string = Command.substring(7,10);
     _Parse_Port_Command(port_string, action_string);
     valid_command = 1;
   }

 sub_string = Command.substring(0,3);

 if (sub_string.compareTo("PWM") == 0)
   {
     command_string = "PWM";
     port_string = Command.substring(4,5);
     action_string = Command.substring(6);
     _Parse_PWM_Command(port_string, action_string);
     valid_command = 1;
   }

 sub_string = Command.substring(0,5);

 if (sub_string.compareTo("COMBO") == 0)
   {
     command_string = "COMBO";
     port_string = Command.substring(6,7);
     action_string = Command.substring(8);
     _Parse_Combo_Command(port_string, action_string);
     valid_command = 1;
   }  

 sub_string = Command.substring(0,4);
   
 if (sub_string.compareTo("CPWM") == 0 )
   {
     command_string = "CPWM";
     port_string = Command.substring(5,6);
     action_string = Command.substring(7,12);
     value_string = Command.substring(12);
     _Parse_CPWM_Command(port_string, action_string, value_string);
     valid_command = 1;
   }

 sub_string = Command.substring(0,5);

 if (sub_string.compareTo("INPUT") == 0)
   {
     command_string = "INPUT";
     port_string = Command.substring(6,7);
     _Parse_Input_Command(port_string);
     valid_command = 1;
   }
   
 sub_string = Command.substring(0,7);

 if (sub_string.compareTo("VERBOSE") == 0)
   {
     command_string = "VERBOSE";
     value_string = Command.substring(8,11);
     _Parse_Verbose_Command(value_string);
     valid_command = 1;
   }    
   
 if (valid_command == 0)
   if (_Verbose == HIGH) {Serial.println("Huh?");}

}

void LegoTechnic::_Parse_Verbose_Command(String Value) {

 int valid_command = 1;
 int action;
 
 if (Value.compareTo("ON") == 0)  
   action = HIGH;
  else
   if (Value.compareTo("OFF") == 0)  
     action = LOW;
    else
     valid_command = 0;
    
  if (valid_command == 1)
    Verbose(action);
   else
    if (_Verbose == HIGH) {Serial.println("VERBOSE Command Syntax Error.");}
   

}

void LegoTechnic::_Parse_Input_Command(String Port) {

 int port_value;
 int return_value;
 
 port_value = Port.toInt();
 return_value = Input_Port(port_value); 
 Serial.println(return_value);
 

}

void LegoTechnic::_Parse_CPWM_Command(String Port, String Action, String Value) {

 char port_value;
 int action_value;
 int value_value;
 int valid_command = 1;
 
 if (Port.compareTo("A") == 0 )
   port_value = 'A';
  else
 if (Port.compareTo("B") == 0 )
   port_value = 'B';
  else
 if (Port.compareTo("C") == 0 )
   port_value = 'C';
  else
   valid_command = 0;;

 if (Action.compareTo("LEFT ") == 0 )
   action_value = COMBO_LEFT;
  else
 if (Action.compareTo("RIGHT") == 0 )
   action_value = COMBO_RIGHT;
  else
   valid_command = 0;
 
 value_value = Value.toInt();
 
 if (valid_command == 1)
   PWM_Combo_Port(port_value, action_value, value_value);
  else
   if (_Verbose == HIGH) {Serial.println("CPWM Command Syntax Error.");}

}

void LegoTechnic::_Parse_PWM_Command(String Port, String Value) {

 int port_value;
 int action_value;
 
 port_value = Port.toInt();
 action_value = Value.toInt();
 
 PWM_Port(port_value, action_value);

}

void LegoTechnic::_Parse_Combo_Command(String Port, String Action) {

 char port_value;
 int action_value;
 
 port_value = Port.toInt();
   
 if (Port.compareTo("A") == 0 )
   port_value = 'A';
  else
 if (Port.compareTo("B") == 0 )
   port_value = 'B';
  else
 if (Port.compareTo("C") == 0 )
   port_value = 'C';
  else
   port_value = 'z';
   
 if (Action.compareTo("LEFT") == 0)
   Combo_Port(port_value, COMBO_LEFT);
  else
   if (Action.compareTo("RIGHT") == 0)
     Combo_Port(port_value, COMBO_RIGHT);
    else
     if (Action.compareTo("OFF") == 0)
       Combo_Port(port_value, COMBO_OFF);
      else 
       if (_Verbose == HIGH) {Serial.println("COMBO Command Syntax Error.");}

}

void LegoTechnic::_Parse_Port_Command(String Port, String Action) {

 int port_value;
 int action_value;
 
 port_value = Port.toInt();
   
 if (Action.compareTo("OFF") == 0)
   Output_Port(port_value, LOW);
  else
   if (Action.compareTo("ON") == 0)
     Output_Port(port_value, HIGH);
    else
     if (_Verbose == HIGH) {Serial.println("PORT Command Syntax Error.");}

}

void LegoTechnic::_Display_Commands() {

 if (_Verbose == HIGH) 
  {
   Serial.println("Commands ...");
   Serial.println("PORT [0..5] ON|OFF");
   Serial.println("PWM [0..5] [0..255]");
   Serial.println("COMBO A|B|C LEFT|RIGHT|OFF");
   Serial.println("CPWM A|B|C LEFT|RIGHT [0..255]");
   Serial.println("INPUT [6..7]");
   Serial.println("?PWM");
   Serial.println("VERBOSE ON|OFF");
  }

}

void LegoTechnic::_Display_PWM_Values() {

 if (_Verbose == HIGH) 
  {
   Serial.print("PORT 0 ");
   Serial.println(Port_PWM[0]);
   Serial.print("PORT 1 ");
   Serial.println(Port_PWM[1]);
   Serial.print("PORT 2 ");
   Serial.println(Port_PWM[2]);
   Serial.print("PORT 3 ");
   Serial.println(Port_PWM[3]);
   Serial.print("PORT 4 ");
   Serial.println(Port_PWM[4]);
   Serial.print("PORT 5 ");
   Serial.println(Port_PWM[5]);
  }

}