-
Notifications
You must be signed in to change notification settings - Fork 1
/
Robot.cpp
81 lines (57 loc) · 2.64 KB
/
Robot.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
//
// Created by Tucker Haydon on 4/25/17.
//
#include "../include/Robot.h"
Robot::Robot() {
/* Start wiring pi */
wiringPiSetup();
/* Create AdafruitPWMServo controller */
this->pwmServoHat = new AdafruitPWMServoHat();
/* Create hardware adapters */
this->leftMotorAdapter = new MotorAdapter(pwmServoHat, LEFT_MOTOR_FORWARD_PIN, LEFT_MOTOR_BACKWARD_PIN);
this->rightMotorAdapter = new MotorAdapter(pwmServoHat, RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN);
this->leftEncoderAdapter = new EncoderAdapter(LEFT_ENCODER_PIN_A, LEFT_ENCODER_PIN_B, ENCODER_TICKS_PER_REVOLUTION, H_LEFT_ENCODER);
this->rightEncoderAdapter = new EncoderAdapter(RIGHT_ENCODER_PIN_A, RIGHT_ENCODER_PIN_B, ENCODER_TICKS_PER_REVOLUTION, H_RIGHT_ENCODER);
/* Create the sensors */
this->leftEncoderSensor = new EncoderSensor();
this->rightEncoderSensor = new EncoderSensor();
/* Create the sensor manager and communicator */
this->sensorManager = new SensorManager();
this->communicator = new Communicator(sensorManager);
/* Register the hardware adapters with the communicator */
this->communicator->registerHardware(H_LEFT_MOTOR, this->leftMotorAdapter);
this->communicator->registerHardware(H_RIGHT_MOTOR, this->rightMotorAdapter);
this->communicator->registerHardware(H_LEFT_ENCODER, this->leftEncoderAdapter);
this->communicator->registerHardware(H_RIGHT_ENCODER, this->rightEncoderAdapter);
/* Register the sensors with the sensor manager */
sensorManager->addSensor(H_LEFT_ENCODER, this->leftEncoderSensor);
sensorManager->addSensor(H_RIGHT_ENCODER, this->rightEncoderSensor);
/* Start the hardware adapters */
this->rightEncoderAdapter->start();
this->leftEncoderAdapter->start();
/* Start the communicator */
this->communicator->start();
/* Create and start the command manager */
this->commander = new CommandManager();
}
void Robot::run() {
double radius = 60; // cm
int duration_ms = 10000; // ms
double rotationRate = 2 * 3.14159 / (duration_ms / 1000.0); // 1 rotation in X seconds
Command* driveArc = new DriveRobotCommand(
this->communicator,
this->leftEncoderSensor,
this->rightEncoderSensor,
radius,
rotationRate,
duration_ms,
WHEEL_DIAMETER,
DRIVETRAIN_DIAMETER);
this->commander->runCommand(driveArc);
while(!driveArc->isFinished());
pwmServoHat->stopAllMotors();
usleep(500000);
pwmServoHat->stopAllMotors();
}
Robot::~Robot() {
}