Lego Technic of the Past Arduino Library

From LVL1
Jump to navigation Jump to search
The printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.

LegoTechnic Header File

/* 
LegoTechnic.h - Library to run Lego Technic 70455 Controller brick with an Arduino Shield.
Created December 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
    
 // Instance Creation Routine
   // Pin Defaults:
   //  Output 0 : Pin 3
   //  Output 1 : Pin 5
   //  Output 2 : Pin 6
   //  Output 3 : Pin 9
   //  Output 4 : Pin 10
   //  Output 5 : Pin 11
   //  Input 6 : Pin Analog 0
   //  Input 7 : Pin Analog 1
 LegoTechnic();

 // Instance Creation Routine
   // Pins Assigned Through Parameters
   // Input Pins Must Be Analog Pins
 LegoTechnic(int P0, int P1, int P2, int P3, int P4, int P5, int P6, int P7);

 // Output Port High/Low Set Routine
   // id (integer): 0 to 6
   // action (literal): PORT_ON, PORT_OFF
   //   PORT_ON sets PWM Value to 255
 void Output_Port(int id, int action);

 // Set Output Port PWM Value
   // id (integer): 0 to 6
   // pwm_value (integer): 0 to 255
 void PWM_Port(int id, int pwm_value);

 // Return Input Port Value
   // id (integer): 0 to 6
   // Analog Pin Returns value between 0 and 1023  
 int Input_Port(int id);

 // Allows Setting Multiple Output Ports
   // COMBO A : Output Ports 0 and 1
   // COMBO B : Output Ports 2 and 3
   // COMBO C : Output Ports 4 and 5
   // id (char): "A", "B", "C"
   // mix (literal): COMBO_OFF, COMBO_LEFT, COMBO_RIGHT, COMBO_BOTH
   //Port values set to 0 or 255
 void Combo_Port(char id, int mix);

 // Allows Setting Value for Multiple Output Ports
   // COMBO A : Output Ports 0 and 1
   // COMBO B : Output Ports 2 and 3
   // COMBO C : Output Ports 4 and 5
   // id (char): "A", "B", "C"
   // mix (literal): COMBO_OFF, COMBO_LEFT, COMBO_RIGHT, COMBO_BOTH
   // pwm_value (integer): 0 to 255
 void PWM_Combo_Port(char id, int mix, int pwm_value); 

 // Sets All Output Port PWM Values to 0
 void Port_Initialize();

 // Returns Output Port PWM Value
   // id (integer): 0 to 6
 int Port_PWM_Value(int id);

 // Enables/Disables Serial Monitor Messages
   // value (integer): 0 to 1 (silent to verbose)
 void Verbose(int value);

 // A Command Parsing Routine
   //Commands ...
   //"PORT [0..5] ON|OFF"
   //"PWM [0..5] [0..255]"
   //"COMBO A|B|C LEFT|RIGHT|OFF"
   //"CPWM A|B|C LEFT|RIGHT [0..255]"
   //"INPUT [6..7]"
   //"?PWM"
   //"VERBOSE ON|OFF"
 void Parse_Command(String Command);

 private:
   int _D0,_D1,_D2,_D3,_D4,_D5,_A6,_A7;
   int _Verbose = HIGH;
   int _Port_PWM[6] = {0, 0, 0, 0, 0, 0};
   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

LEGO Technic Code File

#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 = 255;
   break;
   case PORT_OFF:
     pin_value = 0;
   break;
   default:
     if (_Verbose == HIGH) {Serial.println("Invalid Port Status Identified.");}
     valid_command = 0;
 }
 
 if (valid_command == 1)
   {
     _Port_PWM[id] = pin_value;
     analogWrite(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, 0);
 digitalWrite(_D0, LOW);
 pinMode(_D1, OUTPUT);
 analogWrite(_D1, 0);
 digitalWrite(_D1, LOW);
 pinMode(_D2, OUTPUT);
 analogWrite(_D2, 0);
 digitalWrite(_D2, LOW);
 pinMode(_D3, OUTPUT);
 analogWrite(_D3, 0);
 digitalWrite(_D3, LOW);
 pinMode(_D4, OUTPUT);
 analogWrite(_D4, 0);
 digitalWrite(_D4, LOW);
 pinMode(_D5, OUTPUT);
 analogWrite(_D5, 0);
 digitalWrite(_D5, LOW);

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

int LegoTechnic::Port_PWM_Value(int id)
{
 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 (valid_command == 1)
   return(_Port_PWM[id]);
  else
   return(-1); 
}

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);

 Command.trim();

 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_Value(0));
   Serial.print("PORT 1 ");
   Serial.println(Port_PWM_Value(1));
   Serial.print("PORT 2 ");
   Serial.println(Port_PWM_Value(2));
   Serial.print("PORT 3 ");
   Serial.println(Port_PWM_Value(3));
   Serial.print("PORT 4 ");
   Serial.println(Port_PWM_Value(4));
   Serial.print("PORT 5 ");
   Serial.println(Port_PWM_Value(5));
  }
}

Retrieved from "http://wiki.lvl1.org/index.php?title=Lego_Technic_of_the_Past_Arduino_Library&oldid=8991"