Difference between revisions of "Lego Technic of the Past Arduino Library"

From LVL1
Jump to navigation Jump to search
 
(35 intermediate revisions by the same user not shown)
Line 1: Line 1:
−
==HEADER==
+
==LegoTechnic Header File==
−
 
 
−
/*
 
−
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
+
LegoTechnic.h - Library to run Lego Technic 70455 Controller brick with an Arduino Shield.
 +
Created December 16, 2015
 +
Rights Reserved
 +
*/
  
−
 
+
#ifndef LegoTechnic_h
−
#include "Arduino.h"
+
#define LegoTechnic_h
−
 
+
#include "Arduino.h"
−
 
+
−
class LegoTechnic
+
class LegoTechnic
−
{
+
{
 
   public:
 
   public:
−
 
+
−
#define PORT0_PIN 3
+
  #define PORT0_PIN 3
−
#define PORT1_PIN 5
+
  #define PORT1_PIN 5
−
#define PORT2_PIN 6
+
  #define PORT2_PIN 6
−
#define PORT3_PIN 9
+
  #define PORT3_PIN 9
−
#define PORT4_PIN 10
+
  #define PORT4_PIN 10
−
#define PORT5_PIN 11
+
  #define PORT5_PIN 11
−
 
+
  #define PORT6_ANALOGPIN 0
−
#define PORT6_ANALOGPIN 0
+
  #define PORT7_ANALOGPIN 1
−
#define PORT7_ANALOGPIN 1
+
  #define PORT_ON HIGH
−
 
+
  #define PORT_OFF LOW
−
#define PORT_ON HIGH
+
  #define COMBO_OFF 0
−
#define PORT_OFF LOW
+
  #define COMBO_LEFT 1
−
 
+
  #define COMBO_RIGHT 2
−
#define COMBO_OFF 0
+
  #define COMBO_BOTH 3
−
#define COMBO_LEFT 1
+
   
−
#define COMBO_RIGHT 2
+
  // Instance Creation Routine
−
#define COMBO_BOTH 3
+
    // Pin Defaults:
−
 
+
    //  Output 0 : Pin 3
−
     LegoTechnic();
+
    //  Output 1 : Pin 5
−
     LegoTechnic(int P0, int P1, int P2, int P3, int P4, int P5, int P6, int P7);
+
    //  Output 2 : Pin 6
−
     void Output_Port(int id, int action);
+
    //  Output 3 : Pin 9
−
     void PWM_Port(int id, int pwm_value);
+
    //  Output 4 : Pin 10
−
     int Input_Port(int id);
+
    //  Output 5 : Pin 11
−
     void Combo_Port(char id, int mix);
+
    //  Input 6 : Pin Analog 0
−
     void PWM_Combo_Port(char id, int mix, int pwm_value);
+
     //  Input 7 : Pin Analog 1
−
    void Port_Initialize();
+
  '''LegoTechnic();'''
−
     int Port_PWM[6] = {255, 255, 255, 255, 255, 255};
+
−
     void Verbose(int value);
+
  // Instance Creation Routine
−
     void Parse_Command(String Command);
+
     // 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:
 
   private:
 
     int _D0,_D1,_D2,_D3,_D4,_D5,_A6,_A7;
 
     int _D0,_D1,_D2,_D3,_D4,_D5,_A6,_A7;
 
     int _Verbose = HIGH;
 
     int _Verbose = HIGH;
 +
    int _Port_PWM[6] = {0, 0, 0, 0, 0, 0};
 
     void _Parse_PWM_Command(String Port, String Value);
 
     void _Parse_PWM_Command(String Port, String Value);
 
     void _Parse_Verbose_Command(String Value);
 
     void _Parse_Verbose_Command(String Value);
Line 59: Line 116:
 
     void _Display_Commands();
 
     void _Display_Commands();
 
     void _Display_PWM_Values();
 
     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");
 +
  }
 +
}
 +
 
  
−
#endif
+
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));
 +
  }
 +
}
 
''
 
''

Latest revision as of 22:22, 20 January 2016

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"