/**
control Lego 42100 hub1 und hub2
(c) Copyright 2020 - Cornelius Munz
Released under MIT License
*/
#include "Lpf2Hub.h"
const int potPina = 34;
const int potPinb = 35;
const int potPinc = 33;
//const int potPin = 14;
const int potPine = 32;
const int potPinf = 25;
const int potPing = 26;
const int potPinh = 24;
int tasterstatus = 0;
int taster = 16;
int led = 5;
int ledb = 17;
int mota = 0;
int motb = 0;
int motc = 0;
int mote = 0;
int motf = 0;
int motg = 0;
int moth = 0;
// variable for storing the potentiometer value
int potValuea = 0;
int potValueb = 0;
int potValuec = 0;
//int potValued = 0;
int potValuee = 0;
int potValuef = 0;
int potValueg = 0;
int potValueh = 0;
// create a hub instance
Lpf2Hub myhub1;
byte porta = (byte)ControlPlusHubPort::A;
byte portb = (byte)ControlPlusHubPort::B;
byte portc = (byte)ControlPlusHubPort::D;
Lpf2Hub myhub2;
byte porte = (byte)ControlPlusHubPort::A;
byte portf = (byte)ControlPlusHubPort::B;
byte portg = (byte)ControlPlusHubPort::C;
byte porth = (byte)ControlPlusHubPort::D;
void setup() {
pinMode(taster, INPUT);
pinMode(led, OUTPUT);
pinMode(ledb, OUTPUT);
Serial.begin(115200);
}
// main loop
void loop() {
if (!myhub1.isConnected() && !myhub1.isConnecting())
{
myhub1.init();
}
// connect flow. Search for BLE services and try to connect if the uuid of the hub is found
if (myhub1.isConnecting()) {
myhub1.connectHub();
if (myhub1.isConnected()) {
Serial.println("Connected to HUB");
digitalWrite(led, HIGH);
}
else {
Serial.println("Failed to connect to HUB");
}
}
// controll port A,B,D and shut it down
if (myhub1.isConnected()) {
potValuea = analogRead(potPina);
potValueb = analogRead(potPinb);
potValuec = analogRead(potPinc);
mota = map(potValuea, 0, 4095, -100, 100);
if((mota > -30)&&(mota < 30)) {mota = 0;}
motb = map(potValueb, 0, 4095, 100, -100);
if((motb > -30)&&(motb < 30)) {motb = 0;}
motc = map(potValuec, 0, 4095, 100, -100);
if((motc > -30)&&(motc < 30)) {motc = 0;}
char hubName[] = "myControlPlusHub2";
myhub1.setHubName(hubName);
myhub1.setBasicMotorSpeed(porta, mota);
myhub1.setBasicMotorSpeed(portb, motb);
myhub1.setBasicMotorSpeed(portc, motc);
Serial.println(potValuea);
Serial.println(mota);
Serial.println(potValueb);
Serial.println(motb);
Serial.println(potValuec);
Serial.println(motc);
delay(100);
} else {
Serial.println("ControlPlus hub is disconnected");
digitalWrite(led, LOW);
}
tasterstatus = digitalRead(taster);
if (tasterstatus == HIGH) {
myhub1.shutDownHub();
}
if (!myhub2.isConnected() && !myhub2.isConnecting())
{
myhub2.init();
}
// connect flow. Search for BLE services and try to connect if the uuid of the hub is found
if (myhub2.isConnecting()) {
myhub2.connectHub();
if (myhub2.isConnected()) {
Serial.println("Connected to HUB");
digitalWrite(ledb, HIGH);
}
else {
Serial.println("Failed to connect to HUB");
}
}
// controll port A,B,D and shut it down
if (myhub2.isConnected()) {
potValuee = analogRead(potPine);
potValuef = analogRead(potPinf);
potValueg = analogRead(potPing);
potValueh = analogRead(potPinh);
mote = map(potValuee, 0, 4095, -100, 100);
if((mote > -30)&&(mote < 30)) {mote = 0;}
motf = map(potValuef, 0, 4095, -100, 100);
if((motf > -30)&&(motf < 30)) {motf = 0;}
motg = map(potValueg, 0, 4095, -100, 100);
if((motg > -30)&&(motg < 30)) {motg = 0;}
moth = map(potValueh, 0, 4095, -100, 100);
if((moth > -30)&&(moth < 30)) {moth = 0;}
char hubName[] = "myControlPlusHub";
myhub2.setHubName(hubName);
myhub2.setBasicMotorSpeed(porte, mote);
// myhub2.setBasicMotorSpeed(portf, motf);
//myhub2.setBasicMotorSpeed(portg, motg);
// myhub2.setBasicMotorSpeed(porth, moth);
delay(100);
} else {
Serial.println("ControlPlus hub is disconnected");
digitalWrite(ledb, LOW);
}
tasterstatus = digitalRead(taster);
if (tasterstatus == HIGH) {
myhub2.shutDownHub();
}
} // End of loop