96 lines
3.2 KiB
C++
96 lines
3.2 KiB
C++
#include "Arduino.h"
|
|
|
|
#include "Things/ControlledMotor.h"
|
|
#include "Things/DifferentialDrive.h"
|
|
#include "Things/RelativeEncoder.h"
|
|
#include "Things/TouchSensor.h"
|
|
|
|
#include "Arduino/Things/DRV8833.h"
|
|
#include "Arduino/Things/DigitalInput.h"
|
|
#include "Arduino/Things/UltrasonicSensor.h"
|
|
#include "Arduino/ArduinoUtils.h"
|
|
|
|
#include "Participants/ParticipantUDP.h"
|
|
|
|
#include "configuration.h"
|
|
|
|
#include <iostream>
|
|
|
|
using namespace RoboidControl;
|
|
using namespace RoboidControl::Arduino;
|
|
|
|
ParticipantUDP* localParticipant;
|
|
|
|
DifferentialDrive* bb2b;
|
|
TouchSensor* touchLeft;
|
|
TouchSensor* touchRight;
|
|
|
|
// RelativeEncoder* encoderLeft;
|
|
// RelativeEncoder* encoderRight;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(3000);
|
|
Serial.println("started");
|
|
|
|
StartWifi("serrarens", "192.168.76.44");
|
|
localParticipant = new ParticipantUDP("192.168.77.76");
|
|
|
|
bb2b = new DifferentialDrive();
|
|
touchLeft = new TouchSensor(bb2b);
|
|
touchRight = new TouchSensor(bb2b);
|
|
|
|
// bb2b = new DRV8833::DifferentialDrive(driveConfig);
|
|
// touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
|
|
// touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
|
|
|
|
touchLeft->name = "Left Touch Sensor";
|
|
touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
|
|
touchRight->name = "Right Touch Sensor";
|
|
touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
|
|
|
|
// encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
|
|
// encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
|
|
|
|
// bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
|
|
// bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
|
|
}
|
|
|
|
void loop() {
|
|
|
|
// std::cout << touchLeft->touchedSomething << " | "
|
|
// << touchRight->touchedSomething << std::endl;
|
|
// std::cout << encoderLeft->rotationSpeed << " : "
|
|
// << encoderRight->rotationSpeed << std::endl;
|
|
// std::cout << bb2b->leftWheel->encoder->rotationSpeed
|
|
// << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
|
|
|
|
|
|
// The left wheel turns forward when nothing is touched on the right side
|
|
// and turn backward when the roboid hits something on the right
|
|
float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
|
|
// The right wheel does the same, but instead is controlled by
|
|
// touches on the left side
|
|
float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
|
|
// When both sides are touching something, both wheels will turn backward
|
|
// and the roboid will move backwards
|
|
|
|
bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
|
|
bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
|
|
// std::cout << " " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
|
|
|
|
// float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
|
|
// float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
|
|
// bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
|
|
// std::cout << " " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
|
|
|
|
// std::cout << leftMotor->actualVelocity << std::endl;
|
|
|
|
//Serial.println(".");
|
|
// Update the roboid state
|
|
localParticipant->Update();
|
|
|
|
// and sleep for 100ms
|
|
delay(10);
|
|
}
|