BB2B with HW works
This commit is contained in:
parent
169d842143
commit
31c725b0ca
@ -5,6 +5,46 @@
|
|||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
namespace Arduino {
|
namespace Arduino {
|
||||||
|
|
||||||
|
DRV8833::DRV8833(Configuration config, Participant* participant)
|
||||||
|
: Thing(participant) {
|
||||||
|
this->pinStandby = pinStandby;
|
||||||
|
if (pinStandby != 255)
|
||||||
|
pinMode(pinStandby, OUTPUT);
|
||||||
|
|
||||||
|
this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
|
||||||
|
this->motorA->SetName("Motor A");
|
||||||
|
this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
|
||||||
|
this->motorB->SetName("Motor B");
|
||||||
|
}
|
||||||
|
|
||||||
|
DRV8833::DRV8833(Configuration config, Thing* parent)
|
||||||
|
: DRV8833(config, parent->owner) {
|
||||||
|
this->SetParent(parent);
|
||||||
|
}
|
||||||
|
|
||||||
|
#pragma region Differential drive
|
||||||
|
|
||||||
|
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
|
||||||
|
Participant* owner)
|
||||||
|
: RoboidControl::DifferentialDrive(owner), drv8833(config, owner) {
|
||||||
|
this->leftWheel = this->drv8833.motorA;
|
||||||
|
this->rightWheel = this->drv8833.motorB;
|
||||||
|
}
|
||||||
|
|
||||||
|
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
|
||||||
|
Thing* parent)
|
||||||
|
: RoboidControl::DifferentialDrive(parent), drv8833(config) {}
|
||||||
|
|
||||||
|
void DRV8833::DifferentialDrive::Update(unsigned long currentTimeMs,
|
||||||
|
bool recurse) {
|
||||||
|
RoboidControl::DifferentialDrive::Update(currentTimeMs, recurse);
|
||||||
|
this->drv8833.Update(currentTimeMs, recurse);
|
||||||
|
}
|
||||||
|
|
||||||
|
#pragma endregion Differential drive
|
||||||
|
|
||||||
|
#pragma region Motor
|
||||||
|
|
||||||
#if (ESP32)
|
#if (ESP32)
|
||||||
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
|
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
|
||||||
#endif
|
#endif
|
||||||
@ -13,9 +53,9 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
|
|||||||
unsigned char pinIn1,
|
unsigned char pinIn1,
|
||||||
unsigned char pinIn2,
|
unsigned char pinIn2,
|
||||||
bool reverse)
|
bool reverse)
|
||||||
: Thing(driver->owner) {
|
: Motor(driver->owner) {
|
||||||
this->SetParent(driver);
|
this->SetParent(driver);
|
||||||
|
|
||||||
this->pinIn1 = pinIn1;
|
this->pinIn1 = pinIn1;
|
||||||
this->pinIn2 = pinIn2;
|
this->pinIn2 = pinIn2;
|
||||||
|
|
||||||
@ -33,34 +73,39 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
|
|||||||
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
|
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
this->reverse = reverse;
|
// this->reverse = reverse;
|
||||||
}
|
}
|
||||||
|
|
||||||
void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
|
// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
|
||||||
this->maxRpm = rpm;
|
// this->maxRpm = rpm;
|
||||||
}
|
// }
|
||||||
|
|
||||||
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
|
// void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
|
||||||
Thing::SetAngularVelocity(velocity);
|
void DRV8833Motor::SetTargetSpeed(float motorSpeed) {
|
||||||
// ignoring rotation axis for now.
|
Motor::SetTargetSpeed(motorSpeed);
|
||||||
// Spherical angularVelocity = this->GetAngularVelocity();
|
// Thing::SetAngularVelocity(velocity);
|
||||||
float angularSpeed = velocity.distance; // in degrees/sec
|
// ignoring rotation axis for now.
|
||||||
|
// Spherical angularVelocity = this->GetAngularVelocity();
|
||||||
|
// float angularSpeed = velocity.distance; // in degrees/sec
|
||||||
|
|
||||||
float rpm = angularSpeed / 360 * 60;
|
// float rpm = angularSpeed / 360 * 60;
|
||||||
float motorSpeed = rpm / this->maxRpm;
|
// float motorSpeed = rpm / this->maxRpm;
|
||||||
|
|
||||||
uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
|
uint8_t motorSignal =
|
||||||
// if (direction == RotationDirection::CounterClockwise)
|
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
|
||||||
// speed = -speed;
|
// // if (direction == RotationDirection::CounterClockwise)
|
||||||
// Determine the rotation direction
|
// // speed = -speed;
|
||||||
if (velocity.direction.horizontal.InDegrees() < 0)
|
// // Determine the rotation direction
|
||||||
motorSpeed = -motorSpeed;
|
// if (velocity.direction.horizontal.InDegrees() < 0)
|
||||||
if (this->reverse)
|
// motorSpeed = -motorSpeed;
|
||||||
motorSpeed = -motorSpeed;
|
// if (this->reverse)
|
||||||
|
// motorSpeed = -motorSpeed;
|
||||||
|
|
||||||
// std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm
|
// std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm
|
||||||
// " << rpm
|
// "
|
||||||
// << ", motor signal = " << (int)motorSignal << "\n";
|
// << rpm << ", motor signal = " << (int)motorSignal << "\n";
|
||||||
|
std::cout << "moto speed " << this->name << " = " << motorSpeed
|
||||||
|
<< ", motor signal = " << (int)motorSignal << "\n";
|
||||||
|
|
||||||
#if (ESP32)
|
#if (ESP32)
|
||||||
if (motorSpeed == 0) { // stop
|
if (motorSpeed == 0) { // stop
|
||||||
@ -109,43 +154,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
DRV8833::DRV8833(Participant* participant,
|
#pragma endregion Motor
|
||||||
unsigned char pinAIn1,
|
|
||||||
unsigned char pinAIn2,
|
|
||||||
unsigned char pinBIn1,
|
|
||||||
unsigned char pinBIn2,
|
|
||||||
unsigned char pinStandby,
|
|
||||||
bool reverseA,
|
|
||||||
bool reverseB)
|
|
||||||
: Thing(participant) {
|
|
||||||
this->pinStandby = pinStandby;
|
|
||||||
if (pinStandby != 255)
|
|
||||||
pinMode(pinStandby, OUTPUT);
|
|
||||||
|
|
||||||
this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA);
|
|
||||||
this->motorA->SetName("Motor A");
|
|
||||||
this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
|
|
||||||
this->motorB->SetName("Motor B");
|
|
||||||
}
|
|
||||||
|
|
||||||
DRV8833::DRV8833(Thing* parent,
|
|
||||||
unsigned char pinAIn1,
|
|
||||||
unsigned char pinAIn2,
|
|
||||||
unsigned char pinBIn1,
|
|
||||||
unsigned char pinBIn2,
|
|
||||||
unsigned char pinStandby,
|
|
||||||
bool reverseA,
|
|
||||||
bool reverseB)
|
|
||||||
: DRV8833(parent->owner,
|
|
||||||
pinAIn1,
|
|
||||||
pinAIn2,
|
|
||||||
pinBIn1,
|
|
||||||
pinBIn2,
|
|
||||||
pinStandby,
|
|
||||||
reverseA,
|
|
||||||
reverseB) {
|
|
||||||
this->SetParent(parent);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -1,8 +1,10 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
#include "Participants/IsolatedParticipant.h"
|
||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
#include "Things/DifferentialDrive.h"
|
#include "Things/DifferentialDrive.h"
|
||||||
|
#include "Things/Motor.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
namespace Arduino {
|
namespace Arduino {
|
||||||
@ -11,43 +13,49 @@ class DRV8833Motor;
|
|||||||
|
|
||||||
class DRV8833 : public Thing {
|
class DRV8833 : public Thing {
|
||||||
public:
|
public:
|
||||||
|
struct Configuration {
|
||||||
|
int AIn1;
|
||||||
|
int AIn2;
|
||||||
|
int BIn1;
|
||||||
|
int BIn2;
|
||||||
|
};
|
||||||
|
|
||||||
/// @brief Setup a DRV8833 motor controller
|
/// @brief Setup a DRV8833 motor controller
|
||||||
/// @param pinAIn1 The pin number connected to the AIn1 port
|
DRV8833(Configuration config, Participant* owner = nullptr);
|
||||||
/// @param pinAIn2 The pin number connected to the AIn2 port
|
DRV8833(Configuration config, Thing* parent);
|
||||||
/// @param pinBIn1 The pin number connected to the BIn1 port
|
|
||||||
/// @param pinBIn2 The pin number connceted to the BIn2 port
|
|
||||||
/// @param pinStandby The pin number connected to the standby port, 255
|
|
||||||
/// indicated that the port is not connected
|
|
||||||
/// @param reverseA The forward turning direction of motor A
|
|
||||||
/// @param reverseB The forward turning direction of motor B
|
|
||||||
DRV8833(Participant* participant,
|
|
||||||
unsigned char pinAIn1,
|
|
||||||
unsigned char pinAIn2,
|
|
||||||
unsigned char pinBIn1,
|
|
||||||
unsigned char pinBIn2,
|
|
||||||
unsigned char pinStandby = 255,
|
|
||||||
bool reverseA = false,
|
|
||||||
bool reverseB = false);
|
|
||||||
DRV8833(Thing* parent,
|
|
||||||
unsigned char pinAIn1,
|
|
||||||
unsigned char pinAIn2,
|
|
||||||
unsigned char pinBIn1,
|
|
||||||
unsigned char pinBIn2,
|
|
||||||
unsigned char pinStandby = 255,
|
|
||||||
bool reverseA = false,
|
|
||||||
bool reverseB = false);
|
|
||||||
DRV8833Motor* motorA = nullptr;
|
DRV8833Motor* motorA = nullptr;
|
||||||
DRV8833Motor* motorB = nullptr;
|
DRV8833Motor* motorB = nullptr;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
unsigned char pinStandby = 255;
|
unsigned char pinStandby = 255;
|
||||||
|
|
||||||
|
public:
|
||||||
|
class DifferentialDrive;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// @brief Support for a DRV8833 motor controller
|
#pragma region Differential drive
|
||||||
class DRV8833Motor : public Thing {
|
|
||||||
|
class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
|
||||||
|
public:
|
||||||
|
DifferentialDrive(DRV8833::Configuration config,
|
||||||
|
Participant* participant = nullptr);
|
||||||
|
DifferentialDrive(DRV8833::Configuration config, Thing* parent);
|
||||||
|
|
||||||
|
virtual void Update(unsigned long currentTimeMs,
|
||||||
|
bool recurse = false) override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
DRV8833 drv8833;
|
||||||
|
};
|
||||||
|
|
||||||
|
#pragma endregion Differential drive
|
||||||
|
|
||||||
|
#pragma region Motor
|
||||||
|
|
||||||
|
/// @brief Support for a DRV8833 motor controller
|
||||||
|
class DRV8833Motor : public Motor {
|
||||||
public:
|
public:
|
||||||
/// @brief Motor turning direction
|
|
||||||
enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
|
|
||||||
|
|
||||||
/// @brief Setup the DC motor
|
/// @brief Setup the DC motor
|
||||||
/// @param pinIn1 the pin number for the in1 signal
|
/// @param pinIn1 the pin number for the in1 signal
|
||||||
@ -57,16 +65,16 @@ class DRV8833Motor : public Thing {
|
|||||||
unsigned char pinIn1,
|
unsigned char pinIn1,
|
||||||
unsigned char pinIn2,
|
unsigned char pinIn2,
|
||||||
bool reverse = false);
|
bool reverse = false);
|
||||||
void SetMaxRPM(unsigned int rpm);
|
//void SetMaxRPM(unsigned int rpm);
|
||||||
|
|
||||||
virtual void SetAngularVelocity(Spherical velocity) override;
|
//virtual void SetAngularVelocity(Spherical velocity) override;
|
||||||
|
virtual void SetTargetSpeed(float targetSpeed) override;
|
||||||
bool reverse = false;
|
//bool reverse = false;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
unsigned char pinIn1 = 255;
|
unsigned char pinIn1 = 255;
|
||||||
unsigned char pinIn2 = 255;
|
unsigned char pinIn2 = 255;
|
||||||
unsigned int maxRpm = 200;
|
//unsigned int maxRpm = 200;
|
||||||
|
|
||||||
#if (ESP32)
|
#if (ESP32)
|
||||||
uint8_t in1Ch;
|
uint8_t in1Ch;
|
||||||
@ -75,5 +83,7 @@ class DRV8833Motor : public Thing {
|
|||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#pragma endregion Moto
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -1,39 +1,37 @@
|
|||||||
#include "UltrasonicSensor.h"
|
#include "UltrasonicSensor.h"
|
||||||
|
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
namespace Arduino {
|
namespace Arduino {
|
||||||
|
|
||||||
UltrasonicSensor::UltrasonicSensor(Participant* participant,
|
UltrasonicSensor::UltrasonicSensor(Configuration config,
|
||||||
unsigned char pinTrigger,
|
Participant* participant)
|
||||||
unsigned char pinEcho)
|
|
||||||
: Thing(participant) {
|
: Thing(participant) {
|
||||||
this->pinTrigger = pinTrigger;
|
this->pinTrigger = config.triggerPin;
|
||||||
this->pinEcho = pinEcho;
|
this->pinEcho = config.echoPin;
|
||||||
|
|
||||||
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
|
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
|
||||||
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
|
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
|
||||||
}
|
}
|
||||||
|
|
||||||
UltrasonicSensor::UltrasonicSensor(Thing* parent,
|
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
|
||||||
unsigned char pinTrigger,
|
: UltrasonicSensor(config, parent->owner) {
|
||||||
unsigned char pinEcho)
|
|
||||||
: UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
|
|
||||||
this->SetParent(parent);
|
this->SetParent(parent);
|
||||||
}
|
}
|
||||||
|
|
||||||
float UltrasonicSensor::GetDistance() {
|
float UltrasonicSensor::GetDistance() {
|
||||||
// Start the ultrasonic 'ping'
|
// Start the ultrasonic 'ping'
|
||||||
digitalWrite(pinTrigger, LOW);
|
digitalWrite(pinTrigger, LOW);
|
||||||
delayMicroseconds(5);
|
delayMicroseconds(2);
|
||||||
digitalWrite(pinTrigger, HIGH);
|
digitalWrite(pinTrigger, HIGH);
|
||||||
delayMicroseconds(10);
|
delayMicroseconds(10);
|
||||||
digitalWrite(pinTrigger, LOW);
|
digitalWrite(pinTrigger, LOW);
|
||||||
|
|
||||||
// Measure the duration of the pulse on the echo pin
|
// Measure the duration of the pulse on the echo pin
|
||||||
float duration_us =
|
unsigned long duration_us =
|
||||||
pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds
|
pulseIn(pinEcho, HIGH); // the result is in microseconds
|
||||||
|
|
||||||
// Calculate the distance:
|
// Calculate the distance:
|
||||||
// * Duration should be divided by 2, because the ping goes to the object
|
// * Duration should be divided by 2, because the ping goes to the object
|
||||||
@ -44,14 +42,18 @@ float UltrasonicSensor::GetDistance() {
|
|||||||
// * Now we calculate the distance based on the speed of sound (340 m/s):
|
// * Now we calculate the distance based on the speed of sound (340 m/s):
|
||||||
// distance = duration_sec * 340;
|
// distance = duration_sec * 340;
|
||||||
// * The result calculation is therefore:
|
// * The result calculation is therefore:
|
||||||
this->distance = duration_us / 2 / 1000000 * 340;
|
this->distance = (float)duration_us / 2 / 1000000 * 340;
|
||||||
|
|
||||||
|
// Serial.println(this->distance);
|
||||||
|
|
||||||
|
// std::cout << "US distance " << this->distance << std::endl;
|
||||||
|
|
||||||
// Filter faulty measurements. The sensor can often give values > 30 m which
|
// Filter faulty measurements. The sensor can often give values > 30 m which
|
||||||
// are not correct
|
// are not correct
|
||||||
// if (distance > 30)
|
// if (distance > 30)
|
||||||
// distance = 0;
|
// distance = 0;
|
||||||
|
|
||||||
return distance;
|
return this->distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
|
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
|
||||||
@ -59,11 +61,18 @@ void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
|
|||||||
Thing::Update(currentTimeMs, recursive);
|
Thing::Update(currentTimeMs, recursive);
|
||||||
}
|
}
|
||||||
|
|
||||||
UltrasonicSensor::TouchSensor::TouchSensor(Thing& parent,
|
#pragma region Touch sensor
|
||||||
unsigned char pinTrigger,
|
|
||||||
unsigned char pinEcho)
|
UltrasonicSensor::TouchSensor::TouchSensor(
|
||||||
: RoboidControl::TouchSensor(&parent),
|
UltrasonicSensor::Configuration config,
|
||||||
ultrasonic(&parent, pinTrigger, pinEcho) {
|
Thing& parent)
|
||||||
|
: RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) {
|
||||||
|
this->touchedSomething = false;
|
||||||
|
}
|
||||||
|
UltrasonicSensor::TouchSensor::TouchSensor(
|
||||||
|
UltrasonicSensor::Configuration config,
|
||||||
|
Thing* parent)
|
||||||
|
: RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
|
||||||
this->touchedSomething = false;
|
this->touchedSomething = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -74,5 +83,7 @@ void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs,
|
|||||||
this->ultrasonic.distance <= this->touchDistance);
|
this->ultrasonic.distance <= this->touchDistance);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#pragma region Touch sensor
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -8,16 +8,17 @@ namespace Arduino {
|
|||||||
/// @brief An HC-SR04 ultrasonic distance sensor
|
/// @brief An HC-SR04 ultrasonic distance sensor
|
||||||
class UltrasonicSensor : Thing {
|
class UltrasonicSensor : Thing {
|
||||||
public:
|
public:
|
||||||
|
struct Configuration {
|
||||||
|
int triggerPin;
|
||||||
|
int echoPin;
|
||||||
|
};
|
||||||
|
|
||||||
/// @brief Setup an ultrasonic sensor
|
/// @brief Setup an ultrasonic sensor
|
||||||
/// @param participant The participant to use
|
/// @param participant The participant to use
|
||||||
/// @param pinTrigger The pin number of the trigger signal
|
/// @param pinTrigger The pin number of the trigger signal
|
||||||
/// @param pinEcho The pin number of the echo signal
|
/// @param pinEcho The pin number of the echo signal
|
||||||
UltrasonicSensor(Participant* participant,
|
UltrasonicSensor(Configuration config, Participant* participant);
|
||||||
unsigned char pinTrigger,
|
UltrasonicSensor(Configuration config, Thing* parent);
|
||||||
unsigned char pinEcho);
|
|
||||||
UltrasonicSensor(Thing* parent,
|
|
||||||
unsigned char pinTrigger,
|
|
||||||
unsigned char pinEcho);
|
|
||||||
|
|
||||||
// parameters
|
// parameters
|
||||||
|
|
||||||
@ -47,9 +48,12 @@ class UltrasonicSensor : Thing {
|
|||||||
class TouchSensor;
|
class TouchSensor;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#pragma region Touch sensor
|
||||||
|
|
||||||
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
||||||
public:
|
public:
|
||||||
TouchSensor(Thing& parent, unsigned char pinTrigger, unsigned char pinEcho);
|
TouchSensor(UltrasonicSensor::Configuration config, Thing& parent);
|
||||||
|
TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
|
||||||
|
|
||||||
float touchDistance = 0.2f;
|
float touchDistance = 0.2f;
|
||||||
|
|
||||||
@ -61,5 +65,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
|||||||
UltrasonicSensor ultrasonic;
|
UltrasonicSensor ultrasonic;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#pragma region Touch sensor
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -5,6 +5,7 @@
|
|||||||
#include "Participants/IsolatedParticipant.h"
|
#include "Participants/IsolatedParticipant.h"
|
||||||
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
// #include <iostream>
|
||||||
|
|
||||||
#if defined(ARDUINO)
|
#if defined(ARDUINO)
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
@ -19,12 +20,14 @@ namespace RoboidControl {
|
|||||||
|
|
||||||
#pragma region Init
|
#pragma region Init
|
||||||
|
|
||||||
Thing::Thing(unsigned char thingType)
|
// Thing::Thing(unsigned char thingType)
|
||||||
: Thing(IsolatedParticipant::Isolated(), thingType) {}
|
// : Thing(IsolatedParticipant::Isolated(), thingType) {}
|
||||||
|
|
||||||
Thing::Thing(Participant* owner,
|
Thing::Thing(Participant* owner,
|
||||||
unsigned char thingType,
|
unsigned char thingType,
|
||||||
unsigned char thingId) {
|
unsigned char thingId) {
|
||||||
|
if (owner == nullptr)
|
||||||
|
owner = IsolatedParticipant::Isolated();
|
||||||
this->owner = owner;
|
this->owner = owner;
|
||||||
this->id = thingId;
|
this->id = thingId;
|
||||||
this->type = thingType;
|
this->type = thingType;
|
||||||
@ -245,6 +248,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) {
|
|||||||
this->nameChanged = false;
|
this->nameChanged = false;
|
||||||
|
|
||||||
if (recursive) {
|
if (recursive) {
|
||||||
|
// std::cout << "# chilren: " << (int)this->childCount << std::endl;
|
||||||
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
|
||||||
Thing* child = this->children[childIx];
|
Thing* child = this->children[childIx];
|
||||||
if (child == nullptr)
|
if (child == nullptr)
|
||||||
|
4
Thing.h
4
Thing.h
@ -43,14 +43,14 @@ class Thing {
|
|||||||
|
|
||||||
/// @brief Create a new thing without communication abilities
|
/// @brief Create a new thing without communication abilities
|
||||||
/// @param thingType The type of thing (can use Thing::Type)
|
/// @param thingType The type of thing (can use Thing::Type)
|
||||||
Thing(unsigned char thingType = Type::Undetermined);
|
//Thing(unsigned char thingType = Type::Undetermined);
|
||||||
|
|
||||||
/// @brief Create a new thing for a participant
|
/// @brief Create a new thing for a participant
|
||||||
/// @param owner The owning participant
|
/// @param owner The owning participant
|
||||||
/// @param thingType The type of thing (can use Thing::Type)
|
/// @param thingType The type of thing (can use Thing::Type)
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
/// an ID
|
/// an ID
|
||||||
Thing(Participant* owner,
|
Thing(Participant* owner = nullptr,
|
||||||
unsigned char thingType = Type::Undetermined,
|
unsigned char thingType = Type::Undetermined,
|
||||||
unsigned char thingId = 0);
|
unsigned char thingId = 0);
|
||||||
|
|
||||||
|
@ -2,12 +2,15 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
|
DifferentialDrive::DifferentialDrive(Participant* participant,
|
||||||
|
unsigned char thingId)
|
||||||
|
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {
|
||||||
|
this->leftWheel = new Motor();
|
||||||
|
this->rightWheel = new Motor();
|
||||||
|
}
|
||||||
|
|
||||||
DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId)
|
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId)
|
||||||
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {}
|
: Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
|
||||||
|
|
||||||
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
|
|
||||||
|
|
||||||
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
|
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
|
||||||
float wheelSeparation) {
|
float wheelSeparation) {
|
||||||
@ -24,17 +27,17 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
|
|||||||
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
||||||
}
|
}
|
||||||
|
|
||||||
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
|
// void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
|
||||||
float distance = this->wheelSeparation / 2;
|
// float distance = this->wheelSeparation / 2;
|
||||||
this->leftWheel = leftWheel;
|
// this->leftWheel = leftWheel;
|
||||||
;
|
// ;
|
||||||
if (leftWheel != nullptr)
|
// if (leftWheel != nullptr)
|
||||||
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
|
// this->leftWheel->SetPosition(Spherical(distance, Direction::left));
|
||||||
|
|
||||||
this->rightWheel = rightWheel;
|
// this->rightWheel = rightWheel;
|
||||||
if (rightWheel != nullptr)
|
// if (rightWheel != nullptr)
|
||||||
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
// this->rightWheel->SetPosition(Spherical(distance, Direction::right));
|
||||||
}
|
// }
|
||||||
|
|
||||||
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
|
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
|
||||||
if (this->leftWheel != nullptr)
|
if (this->leftWheel != nullptr)
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
|
#include "Motor.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
@ -9,13 +10,14 @@ namespace RoboidControl {
|
|||||||
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
|
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
|
||||||
class DifferentialDrive : public Thing {
|
class DifferentialDrive : public Thing {
|
||||||
public:
|
public:
|
||||||
/// @brief Create a differential drive without networking support
|
// /// @brief Create a differential drive without networking support
|
||||||
DifferentialDrive();
|
// DifferentialDrive();
|
||||||
/// @brief Create a differential drive with networking support
|
/// @brief Create a differential drive with networking support
|
||||||
/// @param participant The local participant
|
/// @param participant The local participant
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
/// an ID
|
/// an ID
|
||||||
DifferentialDrive(Participant* participant, unsigned char thingId = 0);
|
DifferentialDrive(Participant* participant = nullptr,
|
||||||
|
unsigned char thingId = 0);
|
||||||
/// @brief Create a new child differential drive
|
/// @brief Create a new child differential drive
|
||||||
/// @param parent The parent thing
|
/// @param parent The parent thing
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
@ -45,6 +47,11 @@ class DifferentialDrive : public Thing {
|
|||||||
/// @copydoc RoboidControl::Thing::Update(unsigned long)
|
/// @copydoc RoboidControl::Thing::Update(unsigned long)
|
||||||
virtual void Update(unsigned long currentMs, bool recursive = true) override;
|
virtual void Update(unsigned long currentMs, bool recursive = true) override;
|
||||||
|
|
||||||
|
/// @brief The left wheel
|
||||||
|
Motor* leftWheel = nullptr;
|
||||||
|
/// @brief The right wheel
|
||||||
|
Motor* rightWheel = nullptr;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// @brief The radius of a wheel in meters
|
/// @brief The radius of a wheel in meters
|
||||||
float wheelRadius = 1.0f;
|
float wheelRadius = 1.0f;
|
||||||
@ -53,11 +60,6 @@ class DifferentialDrive : public Thing {
|
|||||||
|
|
||||||
/// @brief Convert revolutions per second to meters per second
|
/// @brief Convert revolutions per second to meters per second
|
||||||
float rpsToMs = 1.0f;
|
float rpsToMs = 1.0f;
|
||||||
|
|
||||||
/// @brief The left wheel
|
|
||||||
Thing* leftWheel = nullptr;
|
|
||||||
/// @brief The right wheel
|
|
||||||
Thing* rightWheel = nullptr;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
|
//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
|
||||||
|
|
||||||
DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
|
DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
|
||||||
: Thing(owner, Type::Switch, thingId) {}
|
: Thing(owner, Type::Switch, thingId) {}
|
||||||
|
@ -8,12 +8,12 @@ namespace RoboidControl {
|
|||||||
class DigitalSensor : public Thing {
|
class DigitalSensor : public Thing {
|
||||||
public:
|
public:
|
||||||
/// @brief Create a digital sensor without communication abilities
|
/// @brief Create a digital sensor without communication abilities
|
||||||
DigitalSensor();
|
//DigitalSensor();
|
||||||
/// @brief Create a digital sensor for a participant
|
/// @brief Create a digital sensor for a participant
|
||||||
/// @param owner The owning participant
|
/// @param owner The owning participant
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
/// an ID
|
/// an ID
|
||||||
DigitalSensor(Participant* owner, unsigned char thingId = 0);
|
DigitalSensor(Participant* owner = nullptr, unsigned char thingId = 0);
|
||||||
/// @brief Create a new child digital sensor
|
/// @brief Create a new child digital sensor
|
||||||
/// @param parent The parent thing
|
/// @param parent The parent thing
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
|
9
Things/Motor.cpp
Normal file
9
Things/Motor.cpp
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
#include "Motor.h"
|
||||||
|
|
||||||
|
RoboidControl::Motor::Motor(Participant* owner) {}
|
||||||
|
|
||||||
|
RoboidControl::Motor::Motor(Thing* parent) {}
|
||||||
|
|
||||||
|
void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) {
|
||||||
|
this->targetSpeed = targetSpeed;
|
||||||
|
}
|
23
Things/Motor.h
Normal file
23
Things/Motor.h
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Thing.h"
|
||||||
|
|
||||||
|
namespace RoboidControl {
|
||||||
|
|
||||||
|
class Motor : public Thing {
|
||||||
|
public:
|
||||||
|
Motor(Participant* owner = nullptr);
|
||||||
|
Motor(Thing* parent);
|
||||||
|
|
||||||
|
/// @brief Motor turning direction
|
||||||
|
enum class Direction { Clockwise = 1, CounterClockwise = -1 };
|
||||||
|
/// @brief The forward turning direction of the motor
|
||||||
|
Direction direction;
|
||||||
|
|
||||||
|
virtual void SetTargetSpeed(float targetSpeed); // -1..0..1
|
||||||
|
|
||||||
|
protected:
|
||||||
|
float targetSpeed = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace RoboidControl
|
@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
|
//TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
|
||||||
|
|
||||||
TouchSensor::TouchSensor(Participant* owner, unsigned char thingId)
|
TouchSensor::TouchSensor(Participant* owner, unsigned char thingId)
|
||||||
: Thing(owner, Thing::Type::TouchSensor, thingId) {}
|
: Thing(owner, Thing::Type::TouchSensor, thingId) {}
|
||||||
|
@ -9,12 +9,12 @@ class TouchSensor : public Thing {
|
|||||||
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
|
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
|
||||||
public:
|
public:
|
||||||
/// @brief Create a touch sensor without communication abilities
|
/// @brief Create a touch sensor without communication abilities
|
||||||
TouchSensor();
|
//TouchSensor();
|
||||||
/// @brief Create a touch sensor for a participant
|
/// @brief Create a touch sensor for a participant
|
||||||
/// @param owner The owning participant
|
/// @param owner The owning participant
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
/// an ID
|
/// an ID
|
||||||
TouchSensor(Participant* owner, unsigned char thingId = 0);
|
TouchSensor(Participant* owner = nullptr, unsigned char thingId = 0);
|
||||||
/// @brief Create a new child touch sensor
|
/// @brief Create a new child touch sensor
|
||||||
/// @param parent The parent thing
|
/// @param parent The parent thing
|
||||||
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
|
Loading…
x
Reference in New Issue
Block a user