Simulator compatibility
This commit is contained in:
parent
a3f0af62c9
commit
5c8d7df31b
@ -1,7 +1,7 @@
|
|||||||
#include <Activation.h>
|
#include "Activation.h"
|
||||||
|
|
||||||
float Activation::HeavisideStep(float inputValue, float bias) {
|
float Activation::HeavisideStep(float inputValue, float bias) {
|
||||||
return (inputValue + bias > 0) ? 1 : 0;
|
return (inputValue + bias > 0) ? 1.0F : 0.0F;
|
||||||
}
|
}
|
||||||
|
|
||||||
float Activation::Linear(float inputValue, float bias, float range) {
|
float Activation::Linear(float inputValue, float bias, float range) {
|
||||||
|
@ -1,4 +1,4 @@
|
|||||||
#include <ControlledMotor.h>
|
#include "ControlledMotor.h"
|
||||||
|
|
||||||
ControlledMotor::ControlledMotor() {}
|
ControlledMotor::ControlledMotor() {}
|
||||||
|
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Encoder.h>
|
#include "Encoder.h"
|
||||||
#include <Motor.h>
|
#include "Motor.h"
|
||||||
|
|
||||||
class ControlledMotor : public Thing {
|
class ControlledMotor : public Thing {
|
||||||
public:
|
public:
|
||||||
@ -18,7 +18,7 @@ class ControlledMotor : public Thing {
|
|||||||
|
|
||||||
void SetTargetVelocity(float rotationsPerSecond);
|
void SetTargetVelocity(float rotationsPerSecond);
|
||||||
float GetActualVelocity() {
|
float GetActualVelocity() {
|
||||||
return rotationDirection * encoder->GetRotationsPerSecond();
|
return (int)rotationDirection * encoder->GetRotationsPerSecond();
|
||||||
} // in rotations per second
|
} // in rotations per second
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
@ -5,9 +5,12 @@
|
|||||||
/// @brief A sensor which can measure the distance the the nearest object
|
/// @brief A sensor which can measure the distance the the nearest object
|
||||||
class DistanceSensor : public Sensor {
|
class DistanceSensor : public Sensor {
|
||||||
public:
|
public:
|
||||||
|
DistanceSensor() { isDistanceSensor = true; }
|
||||||
|
DistanceSensor(float triggerDistance) { isDistanceSensor = true; this->triggerDistance = triggerDistance; }
|
||||||
/// @brief Determine the distance to the nearest object
|
/// @brief Determine the distance to the nearest object
|
||||||
/// @return the measured distance in meters to the nearest object
|
/// @return the measured distance in meters to the nearest object
|
||||||
virtual float GetDistance() = 0;
|
virtual float GetDistance() { return distance; };
|
||||||
|
virtual void SetDistance(float distance) { this->distance = distance; }; // for simulation purposes
|
||||||
|
|
||||||
/// @brief The distance at which ObjectNearby triggers
|
/// @brief The distance at which ObjectNearby triggers
|
||||||
float triggerDistance = 1;
|
float triggerDistance = 1;
|
||||||
@ -21,4 +24,6 @@ class DistanceSensor : public Sensor {
|
|||||||
bool isOff = GetDistance() > triggerDistance;
|
bool isOff = GetDistance() > triggerDistance;
|
||||||
return isOff;
|
return isOff;
|
||||||
}
|
}
|
||||||
|
protected:
|
||||||
|
float distance = 0;
|
||||||
};
|
};
|
||||||
|
@ -1,4 +1,4 @@
|
|||||||
#include <Encoder.h>
|
#include "Encoder.h"
|
||||||
|
|
||||||
volatile unsigned char Encoder::transitionCount = 0;
|
volatile unsigned char Encoder::transitionCount = 0;
|
||||||
|
|
||||||
|
12
Motor.cpp
12
Motor.cpp
@ -1,7 +1,7 @@
|
|||||||
#include <Motor.h>
|
#include "Motor.h"
|
||||||
|
|
||||||
Motor::Motor() {
|
Motor::Motor() {
|
||||||
this->isSensor = false;
|
this->isMotor = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
|
// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
|
||||||
@ -23,3 +23,11 @@ Motor::Motor() {
|
|||||||
// analogWrite(pinIn1, speed);
|
// analogWrite(pinIn1, speed);
|
||||||
// analogWrite(pinIn2, 255 - speed);
|
// analogWrite(pinIn2, 255 - speed);
|
||||||
// }
|
// }
|
||||||
|
|
||||||
|
float Motor::GetSpeed() {
|
||||||
|
return this->currentSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Motor::SetSpeed(float speed) {
|
||||||
|
this->currentSpeed = speed;
|
||||||
|
}
|
2
Motor.h
2
Motor.h
@ -12,7 +12,7 @@ class Motor : public Thing {
|
|||||||
/// @brief Set the turning direction of the motor
|
/// @brief Set the turning direction of the motor
|
||||||
// void SetDirection(Direction direction);
|
// void SetDirection(Direction direction);
|
||||||
|
|
||||||
virtual void SetSpeed(float speed) = 0;
|
virtual void SetSpeed(float speed);
|
||||||
float GetSpeed();
|
float GetSpeed();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
@ -1,4 +1,4 @@
|
|||||||
#include <Placement.h>
|
#include "Placement.h"
|
||||||
|
|
||||||
Placement::Placement() {
|
Placement::Placement() {
|
||||||
this->position = Vector3::zero;
|
this->position = Vector3::zero;
|
||||||
@ -15,11 +15,11 @@ Placement::Placement(Vector2 direction, Sensor* thing) {
|
|||||||
this->thing = thing;
|
this->thing = thing;
|
||||||
}
|
}
|
||||||
|
|
||||||
Placement::Placement(Vector3 position, Sensor* thing) {
|
//Placement::Placement(Vector3 position, Sensor* thing) {
|
||||||
this->position = position;
|
// this->position = position;
|
||||||
this->direction = Vector2::zero;
|
// this->direction = Vector2::zero;
|
||||||
this->thing = thing;
|
// this->thing = thing;
|
||||||
}
|
//}
|
||||||
|
|
||||||
Placement::Placement(Vector3 position, Motor* thing) {
|
Placement::Placement(Vector3 position, Motor* thing) {
|
||||||
this->position = position;
|
this->position = position;
|
||||||
@ -27,13 +27,13 @@ Placement::Placement(Vector3 position, Motor* thing) {
|
|||||||
this->thing = thing;
|
this->thing = thing;
|
||||||
}
|
}
|
||||||
|
|
||||||
Placement::Placement(Vector3 position, ControlledMotor* thing) {
|
//Placement::Placement(Vector3 position, ControlledMotor* thing) {
|
||||||
this->position = position;
|
// this->position = position;
|
||||||
this->direction = Vector2::zero;
|
// this->direction = Vector2::zero;
|
||||||
this->thing = thing;
|
// this->thing = thing;
|
||||||
}
|
//}
|
||||||
|
//
|
||||||
Placement::Placement(Thing* thing, Vector3 position) {
|
//Placement::Placement(Thing* thing, Vector3 position) {
|
||||||
this->thing = thing;
|
// this->thing = thing;
|
||||||
this->position = position;
|
// this->position = position;
|
||||||
}
|
//}
|
18
Placement.h
18
Placement.h
@ -1,21 +1,21 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <ControlledMotor.h>
|
#include "ControlledMotor.h"
|
||||||
#include <Motor.h>
|
#include "Motor.h"
|
||||||
#include <Thing.h>
|
#include "Thing.h"
|
||||||
#include <Vector2.h>
|
#include "Vector2.h"
|
||||||
#include <Vector3.h>
|
#include "Vector3.h"
|
||||||
#include "Sensor.h"
|
#include "DistanceSensor.h"
|
||||||
|
|
||||||
class Placement {
|
class Placement {
|
||||||
public:
|
public:
|
||||||
Placement();
|
Placement();
|
||||||
Placement(Vector2 direction, Sensor* sensor);
|
Placement(Vector2 direction, Sensor* sensor);
|
||||||
|
|
||||||
Placement(Vector3 position, Sensor* sensor);
|
//Placement(Vector3 position, Sensor* sensor);
|
||||||
Placement(Vector3 position, Motor* motor);
|
Placement(Vector3 position, Motor* motor);
|
||||||
Placement(Vector3 position, ControlledMotor* motor);
|
/*Placement(Vector3 position, ControlledMotor* motor);
|
||||||
Placement(Thing* thing, Vector3 position);
|
Placement(Thing* thing, Vector3 position);*/
|
||||||
|
|
||||||
Placement* parent = nullptr;
|
Placement* parent = nullptr;
|
||||||
Placement** children = nullptr;
|
Placement** children = nullptr;
|
||||||
|
@ -1,21 +1,51 @@
|
|||||||
#include <ControlledMotor.h>
|
#include "ControlledMotor.h"
|
||||||
#include <Propulsion.h>
|
#include "Propulsion.h"
|
||||||
|
|
||||||
#include <FloatSingle.h>
|
#include "FloatSingle.h"
|
||||||
|
|
||||||
Propulsion::Propulsion() {
|
Propulsion::Propulsion() {
|
||||||
this->motors = nullptr;
|
this->placement = nullptr;
|
||||||
this->motorCount = 0;
|
this->motorCount = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
|
//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
|
||||||
this->motors = motors;
|
// this->palce = motors;
|
||||||
this->motorCount = motorCount;
|
// this->motorCount = motorCount;
|
||||||
|
//}
|
||||||
|
|
||||||
|
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
|
||||||
|
//this->placement = motors;
|
||||||
|
//this->motorCount = motorCount;
|
||||||
|
this->motorCount = 0;
|
||||||
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
||||||
|
Thing* thing = things[thingIx].thing;
|
||||||
|
if (thing->isMotor)
|
||||||
|
motorCount++;
|
||||||
|
}
|
||||||
|
this->placement = new Placement[motorCount];
|
||||||
|
|
||||||
|
unsigned int motorIx = 0;
|
||||||
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
||||||
|
Thing* thing = things[thingIx].thing;
|
||||||
|
if (thing->isMotor)
|
||||||
|
this->placement[motorIx++] = things[thingIx];
|
||||||
}
|
}
|
||||||
|
|
||||||
void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) {
|
}
|
||||||
this->placement = motors;
|
|
||||||
this->motorCount = motorCount;
|
unsigned int Propulsion::GetMotorCount() {
|
||||||
|
return this->motorCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
Motor* Propulsion::GetMotor(unsigned int motorId) {
|
||||||
|
if (motorId >= this->motorCount)
|
||||||
|
return nullptr;
|
||||||
|
|
||||||
|
Thing* thing = this->placement[motorId].thing;
|
||||||
|
if (thing->isMotor)
|
||||||
|
return (Motor*)thing;
|
||||||
|
|
||||||
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Propulsion::Update() {
|
void Propulsion::Update() {
|
||||||
@ -25,18 +55,18 @@ void Propulsion::Update() {
|
|||||||
// lastMillis = curMillis;
|
// lastMillis = curMillis;
|
||||||
|
|
||||||
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
||||||
MotorPlacement placement = motors[motorIx];
|
//Placement placement = placement[motorIx];
|
||||||
// placement.controlledMotor->Update(timeStep);
|
// placement.controlledMotor->Update(timeStep);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
|
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
|
||||||
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
||||||
Motor* motor = motors[motorIx].motor;
|
Motor* motor = (Motor*) placement[motorIx].thing;
|
||||||
if (motor == nullptr)
|
if (motor == nullptr)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
float xPosition = motors[motorIx].position.x;
|
float xPosition = placement[motorIx].position.x;
|
||||||
if (xPosition < 0)
|
if (xPosition < 0)
|
||||||
motor->SetSpeed(leftSpeed);
|
motor->SetSpeed(leftSpeed);
|
||||||
else if (xPosition > 0)
|
else if (xPosition > 0)
|
||||||
@ -46,11 +76,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
|
|||||||
|
|
||||||
void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
|
void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
|
||||||
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
||||||
MotorPlacement placement = motors[motorIx];
|
//Placement placement = placement[motorIx];
|
||||||
if (placement.position.x < 0)
|
//if (placement.position.x < 0)
|
||||||
placement.controlledMotor->SetTargetVelocity(leftVelocity);
|
// placement.controlledMotor->SetTargetVelocity(leftVelocity);
|
||||||
else if (placement.position.x > 0)
|
//else if (placement.position.x > 0)
|
||||||
placement.controlledMotor->SetTargetVelocity(rightVelocity);
|
// placement.controlledMotor->SetTargetVelocity(rightVelocity);
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
25
Propulsion.h
25
Propulsion.h
@ -1,16 +1,16 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <ControlledMotor.h>
|
#include "ControlledMotor.h"
|
||||||
#include <Placement.h>
|
#include "Placement.h"
|
||||||
#include <Vector2.h>
|
#include "Vector2.h"
|
||||||
|
|
||||||
#include <list>
|
#include <list>
|
||||||
|
|
||||||
struct MotorPlacement {
|
//struct MotorPlacement {
|
||||||
Motor* motor;
|
// Motor* motor;
|
||||||
ControlledMotor* controlledMotor;
|
// ControlledMotor* controlledMotor;
|
||||||
Vector2 position;
|
// Vector2 position;
|
||||||
};
|
//};
|
||||||
|
|
||||||
class Propulsion {
|
class Propulsion {
|
||||||
public:
|
public:
|
||||||
@ -19,9 +19,12 @@ class Propulsion {
|
|||||||
|
|
||||||
void Update();
|
void Update();
|
||||||
|
|
||||||
void AddMotors(MotorPlacement* motors, unsigned int motorCount);
|
//void AddMotors(MotorPlacement* motors, unsigned int motorCount);
|
||||||
void AddMotors(Placement* motors, unsigned int motorCount);
|
void AddMotors(Placement* motors, unsigned int motorCount);
|
||||||
|
|
||||||
|
unsigned int GetMotorCount();
|
||||||
|
Motor* GetMotor(unsigned int motorIx);
|
||||||
|
|
||||||
void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
|
void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
|
||||||
void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
|
void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
|
||||||
|
|
||||||
@ -33,8 +36,8 @@ class Propulsion {
|
|||||||
void SetLinearSpeed(Vector3 direction);
|
void SetLinearSpeed(Vector3 direction);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
unsigned long lastMillis;
|
//unsigned long lastMillis;
|
||||||
MotorPlacement* motors = nullptr;
|
//MotorPlacement* motors = nullptr;
|
||||||
Placement* placement = nullptr;
|
Placement* placement = nullptr;
|
||||||
unsigned int motorCount = 0;
|
unsigned int motorCount = 0;
|
||||||
};
|
};
|
||||||
|
@ -1,4 +1,4 @@
|
|||||||
#include <Roboid.h>
|
#include "Roboid.h"
|
||||||
|
|
||||||
Roboid::Roboid() {
|
Roboid::Roboid() {
|
||||||
this->configuration = nullptr;
|
this->configuration = nullptr;
|
||||||
@ -10,4 +10,5 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
|
|||||||
this->thingCount = thingCount;
|
this->thingCount = thingCount;
|
||||||
|
|
||||||
sensing.AddSensors(configuration, thingCount);
|
sensing.AddSensors(configuration, thingCount);
|
||||||
|
propulsion.AddMotors(configuration, thingCount);
|
||||||
}
|
}
|
8
Roboid.h
8
Roboid.h
@ -1,9 +1,9 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Activation.h>
|
#include "Activation.h"
|
||||||
#include <Placement.h>
|
#include "Placement.h"
|
||||||
#include <Propulsion.h>
|
#include "Propulsion.h"
|
||||||
#include <Sensing.h>
|
#include "Sensing.h"
|
||||||
|
|
||||||
class Roboid {
|
class Roboid {
|
||||||
public:
|
public:
|
||||||
|
21
Sensing.cpp
21
Sensing.cpp
@ -1,6 +1,6 @@
|
|||||||
#include <DistanceSensor.h>
|
#include "DistanceSensor.h"
|
||||||
#include <Sensing.h>
|
#include "Sensing.h"
|
||||||
#include <Switch.h>
|
//#include <Switch.h>
|
||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
@ -41,6 +41,21 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unsigned int Sensing::GetSensorCount() {
|
||||||
|
return this->sensorCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
Sensor* Sensing::GetSensor(unsigned int sensorId) {
|
||||||
|
if (sensorId >= this->sensorCount)
|
||||||
|
return nullptr;
|
||||||
|
|
||||||
|
Thing* thing = this->sensorPlacements[sensorId].thing;
|
||||||
|
if (thing->isSensor)
|
||||||
|
return (Sensor*)thing;
|
||||||
|
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
float Sensing::DistanceForward(float angle) {
|
float Sensing::DistanceForward(float angle) {
|
||||||
float minDistance = INFINITY;
|
float minDistance = INFINITY;
|
||||||
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Placement.h>
|
#include "Placement.h"
|
||||||
|
|
||||||
#include <list>
|
#include <list>
|
||||||
|
|
||||||
@ -27,6 +27,9 @@ class Sensing {
|
|||||||
// void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
|
// void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
|
||||||
void AddSensors(Placement* sensors, unsigned int sensorCount);
|
void AddSensors(Placement* sensors, unsigned int sensorCount);
|
||||||
|
|
||||||
|
unsigned int GetSensorCount();
|
||||||
|
Sensor* GetSensor(unsigned int sensorId);
|
||||||
|
|
||||||
float DistanceForward(float angle = 90);
|
float DistanceForward(float angle = 90);
|
||||||
|
|
||||||
/// @brief Distance to the closest object on the left
|
/// @brief Distance to the closest object on the left
|
||||||
|
2
Sensor.h
2
Sensor.h
@ -1,6 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Thing.h>
|
#include "Thing.h"
|
||||||
|
|
||||||
class Sensor : public Thing {
|
class Sensor : public Thing {
|
||||||
public:
|
public:
|
||||||
|
Loading…
x
Reference in New Issue
Block a user