56 lines
1.7 KiB
C++
56 lines
1.7 KiB
C++
// #if GTEST
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "../DifferentialDrive.h"
|
|
#include "../DistanceSensor.h"
|
|
#include "../Roboid.h"
|
|
#include "../VectorAlgebra/Angle.h"
|
|
|
|
TEST(BB2B, Basics) {
|
|
Motor *motorLeft = new Motor();
|
|
Motor *motorRight = new Motor();
|
|
|
|
DistanceSensor *sensorLeft = new DistanceSensor();
|
|
sensorLeft->position.angle = -30;
|
|
DistanceSensor *sensorRight = new DistanceSensor();
|
|
sensorRight->position.angle = 30;
|
|
|
|
Sensor *sensors[] = {sensorLeft, sensorRight};
|
|
|
|
Perception *perception = new Perception(sensors, 2);
|
|
|
|
DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
|
|
propulsion->SetDimensions(
|
|
1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
|
|
1);
|
|
|
|
Roboid *roboid = new Roboid(perception, propulsion);
|
|
|
|
bool obstacleLeft = roboid->perception->ObjectNearby(-30);
|
|
bool obstacleRight = roboid->perception->ObjectNearby(30);
|
|
|
|
EXPECT_FALSE(obstacleLeft);
|
|
EXPECT_FALSE(obstacleRight);
|
|
|
|
float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
|
|
float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
|
|
|
|
EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
|
|
|
|
DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
|
|
diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
|
|
|
float leftActualSpeed = motorLeft->GetActualSpeed();
|
|
float rightActualSpeed = motorRight->GetActualSpeed();
|
|
|
|
EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
|
|
EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
|
|
|
|
Polar velocity =
|
|
diffDrive->GetVelocity(); // this depends on the wheel diameter.
|
|
EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
|
|
EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
|
|
}
|
|
// #endif
|