52 lines
1.0 KiB
C++
52 lines
1.0 KiB
C++
#include <Adafruit_MotorShield.h>
|
|
#include "simdata.h"
|
|
|
|
#define BYTE_SIZE sizeof(SimData)
|
|
#define KPHTOMPH .621317
|
|
|
|
|
|
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
|
|
|
|
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
|
|
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(3);
|
|
|
|
SimData sd;
|
|
int velocity = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
if (!AFMS.begin()) {
|
|
Serial.println("Could not find Motor Shield. Check wiring.");
|
|
while (1);
|
|
}
|
|
sd.rpms = 0;
|
|
sd.maxrpm = 6500;
|
|
sd.altitude = 10;
|
|
sd.pulses = 40000;
|
|
sd.velocity = 10;
|
|
|
|
myMotor1->setSpeed(0);
|
|
myMotor1->run(FORWARD);
|
|
|
|
myMotor2->setSpeed(0);
|
|
myMotor2->run(FORWARD);
|
|
}
|
|
|
|
void loop() {
|
|
char buff[BYTE_SIZE];
|
|
|
|
if (Serial.available() >= BYTE_SIZE)
|
|
{
|
|
Serial.readBytes(buff, BYTE_SIZE);
|
|
memcpy(&sd, &buff, BYTE_SIZE);
|
|
velocity = sd.velocity;
|
|
}
|
|
int v = ceil(velocity * KPHTOMPH);
|
|
if (v >= 255)
|
|
{
|
|
v = 255;
|
|
}
|
|
myMotor1->setSpeed(v);
|
|
myMotor2->setSpeed(v);
|
|
}
|