Skip to content

Commit

Permalink
[#585] Add Nic's gps code back and include necessary files
Browse files Browse the repository at this point in the history
  • Loading branch information
@OZiad
OZiad committed Dec 19, 2023
1 parent 3e8ae49 commit a0d3360
Showing 1 changed file with 153 additions and 88 deletions.
241 changes: 153 additions & 88 deletions robot/rover/MobilePlatform/MobilePlatform.ino
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "APA102.h"

#include <SparkFun_I2C_GPS_Arduino_Library.h>
#include <TinyGPS++.h>
#include "Rover.h"
#include "commands/WheelsCommandCenter.h"

Expand All @@ -15,132 +17,195 @@ const uint8_t TRANSMIT_PIN = 14;

internal_comms::CommandCenter* commandCenter = new WheelsCommandCenter();

I2CGPS myI2CGPS;
TinyGPSPlus myGPS;
IntervalTimer gpsTimer;

void attachMotors();
void attachServos();

void blink(int dur){
digitalWrite(LED_BUILTIN,HIGH);
delay(dur);
digitalWrite(LED_BUILTIN,LOW);
delay(dur);
void blink(int dur)
{
digitalWrite(LED_BUILTIN, HIGH);
delay(dur);
digitalWrite(LED_BUILTIN, LOW);
delay(dur);
}

void gpsIRQ()
{
noInterrupts()
if (Rover::systemStatus.is_gps_enabled)
{
if (myI2CGPS.available())
{
myGPS.encode(myI2CGPS.read());
}

if (myGPS.time.isUpdated() && myGPS.location.isValid()) //Check to see if new GPS info is available
{
blink(100);
double lat = myGPS.location.lat();
double lng = myGPS.location.lng();

uint8_t* lat_buffer = nullptr;
uint8_t* lng_buffer = nullptr;

double2bytes(lat_buffer, lat);
double2bytes(lat_buffer, lng);

byte data_buffer[16];
memcpy(data_buffer, lat_buffer, 8);
memcpy(data_buffer + 8, lng_buffer, 8);

auto msg = commandCenter->createMessage(COMMAND_SEND_GPS, sizeof(data_buffer),
data_buffer);
commandCenter->queueMessage(*msg);
}
}
interrupts()
}

void setup() {
pinMode(LED_BUILTIN,OUTPUT);
pinMode(V_SENSE_PIN, INPUT);
pinMode(ACTIVITY_CLK, OUTPUT);
pinMode(ACTIVITY_MOSI, OUTPUT);
void setup()
{
pinMode(LED_BUILTIN, OUTPUT);
pinMode(V_SENSE_PIN, INPUT);
pinMode(ACTIVITY_CLK, OUTPUT);
pinMode(ACTIVITY_MOSI, OUTPUT);

commandCenter->startSerial( RX_TEENSY_3_6_PIN,TX_TEENSY_3_6_PIN, ENABLE_PIN, TRANSMIT_PIN);
commandCenter->startSerial(RX_TEENSY_3_6_PIN, TX_TEENSY_3_6_PIN, ENABLE_PIN, TRANSMIT_PIN);

attachMotors();
attachServos();
if (!myI2CGPS.begin())
{
blink(5000);
}

blink(500);
attachMotors();
attachServos();

// Here different parameters of how the system should behave can be set
Rover::systemStatus.is_throttle_timeout_enabled = true;
}
blink(500);

void loop() {
if (Serial.available() > 0) {
commandCenter->readCommand();
}
Rover::handleActivityLight();
// Sort of un-used at the moment, but this can periodically transmit messages to the OBC which can define the status
// of the rover.
// Here different parameters of how the system should behave can be set
Rover::systemStatus.is_throttle_timeout_enabled = true;
Rover::systemStatus.is_gps_enabled = true;

// The rover will update its own current velocity according to a time interval which is measured from when it
// last moved (in moveRover() )
if ((millis() - Rover::systemStatus.last_velocity_adjustment) > ACCELERATION_RATE) {
Rover::updateWheelVelocities();
}
// If the rover has not been commanded to move which a time interval, it must be stopped (security risk).
if (Rover::systemStatus.is_throttle_timeout_enabled &&
((millis() - Rover::systemStatus.last_move) > ROVER_MOVE_TIMEOUT)) {
Rover::decelerateRover();
}
gpsTimer.begin(gpsIRQ, 1000000);
}

// In case there are any messages queued in the transmit buffer, they should be sent.
commandCenter->sendMessage();
void loop()
{
if (Serial.available() > 0)
{
commandCenter->readCommand();
}
Rover::handleActivityLight();
// Sort of un-used at the moment, but this can periodically transmit messages to the OBC which can define the status
// of the rover.

// The rover will update its own current velocity according to a time interval which is measured from when it
// last moved (in moveRover() )
if ((millis() - Rover::systemStatus.last_velocity_adjustment) > ACCELERATION_RATE)
{
Rover::updateWheelVelocities();
}
// If the rover has not been commanded to move which a time interval, it must be stopped (security risk).
if (Rover::systemStatus.is_throttle_timeout_enabled &&
((millis() - Rover::systemStatus.last_move) > ROVER_MOVE_TIMEOUT))
{
Rover::decelerateRover();
}

// In case there are any messages queued in the transmit buffer, they should be sent.
commandCenter->sendMessage();
}

void attachServos(){
Rover::attachServo(CENTER_BACK_1_SERVO,CB_1_SERVO);
Rover::attachServo(CENTER_BACK_2_SERVO,CB_2_SERVO);
void attachServos()
{
Rover::attachServo(CENTER_BACK_1_SERVO, CB_1_SERVO);
Rover::attachServo(CENTER_BACK_2_SERVO, CB_2_SERVO);
}

void attachMotors(){
Motor::attachMotor(FRONT_RIGHT,M1_FR_DIR,M1_FR_PWM);
Motor::attachMotor(MIDDLE_RIGHT,M2_MR_DIR,M2_MR_PWM);
Motor::attachMotor(REAR_RIGHT,M3_RR_DIR,M3_RR_PWM);
void attachMotors()
{
Motor::attachMotor(FRONT_RIGHT, M1_FR_DIR, M1_FR_PWM);
Motor::attachMotor(MIDDLE_RIGHT, M2_MR_DIR, M2_MR_PWM);
Motor::attachMotor(REAR_RIGHT, M3_RR_DIR, M3_RR_PWM);

Motor::attachMotor(FRONT_LEFT,M4_FL_DIR,M4_FL_PWM);
Motor::attachMotor(MIDDLE_LEFT,M5_ML_DIR,M5_ML_PWM);
Motor::attachMotor(REAR_LEFT,M6_RL_DIR,M6_RL_PWM);
Motor::attachMotor(FRONT_LEFT, M4_FL_DIR, M4_FL_PWM);
Motor::attachMotor(MIDDLE_LEFT, M5_ML_DIR, M5_ML_PWM);
Motor::attachMotor(REAR_LEFT, M6_RL_DIR, M6_RL_PWM);
}
void WheelsCommandCenter::stopMotors() {
Rover::stopMotors();
void WheelsCommandCenter::stopMotors()
{
Rover::stopMotors();
}

void WheelsCommandCenter::moveRover(const float & linear_y,const float & omega_z) {
Rover::moveRover(linear_y,omega_z);
void WheelsCommandCenter::moveRover(const float& linear_y, const float& omega_z)
{
Rover::moveRover(linear_y, omega_z);
}

void WheelsCommandCenter::moveServo(const uint8_t & servoID, const uint8_t & angle) {
Rover::moveServo((ServoNames)servoID,angle);
void WheelsCommandCenter::moveServo(const uint8_t& servoID, const uint8_t& angle)
{
Rover::moveServo((ServoNames)servoID, angle);
}
void WheelsCommandCenter::moveWheel(const uint8_t& wheelNumber,const uint8_t& direction,const uint8_t& speed) {
Rover::moveWheel((MotorNames)wheelNumber,direction,speed);
void WheelsCommandCenter::moveWheel(const uint8_t& wheelNumber, const uint8_t& direction, const uint8_t& speed)
{
Rover::moveWheel((MotorNames)wheelNumber, direction, speed);
}
void WheelsCommandCenter::getLinearVelocity(void) {
const float linear_velocity = Rover::roverState.linear_velocity;
uint8_t buffer[4];
float2bytes(buffer,linear_velocity);
internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
void WheelsCommandCenter::getLinearVelocity(void)
{
const float linear_velocity = Rover::roverState.linear_velocity;
uint8_t buffer[4];
float2bytes(buffer, linear_velocity);
internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
}

void WheelsCommandCenter::getRotationalVelocity(void) {
const float rotational_velocity = Rover::roverState.rotational_velocity;
uint8_t buffer[4];
float2bytes(buffer,rotational_velocity);
internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
void WheelsCommandCenter::getRotationalVelocity(void)
{
const float rotational_velocity = Rover::roverState.rotational_velocity;
uint8_t buffer[4];
float2bytes(buffer, rotational_velocity);
internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
}

void WheelsCommandCenter::pingWheels(void) {
internal_comms::Message* message = commandCenter->createMessage(1, 0, nullptr);
commandCenter->sendMessage(*message);
void WheelsCommandCenter::pingWheels(void)
{
internal_comms::Message* message = commandCenter->createMessage(1, 0, nullptr);
commandCenter->sendMessage(*message);
}

// Defines functions from WheelsCommandCenter.h
void WheelsCommandCenter::handleBlink(uint8_t on) {
Rover::setActivityColor(ACTIVITY_DEFAULT_COLOR);
Rover::setActivityBlinking(on);
void WheelsCommandCenter::handleBlink(uint8_t on)
{
Rover::setActivityColor(ACTIVITY_DEFAULT_COLOR);
Rover::setActivityBlinking(on);
}

void WheelsCommandCenter::handleBlinkColor(uint8_t r, uint8_t g, uint8_t b) {
Rover::setActivityColor(r, g, b);
Rover::setActivityBlinking(1);
void WheelsCommandCenter::handleBlinkColor(uint8_t r, uint8_t g, uint8_t b)
{
Rover::setActivityColor(r, g, b);
Rover::setActivityBlinking(1);
}

void WheelsCommandCenter::getBatteryVoltage() {
void WheelsCommandCenter::getBatteryVoltage()
{

//convert to 3.3V reference from analog values (3.3/1023=0.003225806)
auto vsense = (float)analogRead(V_SENSE_PIN) * 0.003225806;
//convert to 3.3V reference from analog values (3.3/1023=0.003225806)
auto vsense = (float)analogRead(V_SENSE_PIN) * 0.003225806;

float vbatt = vsense * 6.0;
auto* buffer = (uint8_t *) malloc(4);
//uint8_t* buffer;
float vbatt = vsense * 6.0;
auto* buffer = (uint8_t*)malloc(4);
//uint8_t* buffer;

float2bytes(buffer,vbatt);
float2bytes(buffer, vbatt);

internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
free(buffer);
internal_comms::Message* message = commandCenter->createMessage(
COMMAND_GET_BATTERY_VOLTAGE, sizeof(buffer), buffer);
commandCenter->sendMessage(*message);
free(buffer);
}

0 comments on commit a0d3360

Please sign in to comment.