Robot.cpp

#include "WPILib.h"
#include "Talon.h"

#include "Simulation/Model.h"
#include "Simulation/Mutator.h"
#include "Simulation/Data/PWMData.h"

#include <cstdio>
#include <cmath>

using namespace frc;

// In reality, this would be inside of WPILib (EncoderData.h), but I haven't gotten around
// to that yet.
typedef struct {
	long int ticks;
} FakeEncoderData;

// This motor model is a (dumb) approximation of a DC motor
struct MotorState { double angular_velocity; /* RPM */ };
class MotorModel : public sim::Model<struct MotorState, sim::PWMData> {
public:
	MotorModel(double rpm) : _rpm(rpm) {}
	struct MotorState calculate(struct MotorState state, const sim::PWMData pwm, const double dt) const override {
		state.angular_velocity = pwm.speed * _rpm;	// Free Speed @ 100% Throttle (no load)
		return state;
	}
private:
	double _rpm;
};

// This encoder model takes motor inputs and converts them to encoder readings (since we rely on dt)
// It's, once again, a pretty dumb representation.
struct EncoderState { long int ticks; };
class RotaryEncoderModel : public sim::Model<struct EncoderState, struct MotorState> {
public:
	RotaryEncoderModel(double ticksPerRev) : _ticksPerRev(ticksPerRev) {}
	struct EncoderState calculate(struct EncoderState state, const struct MotorState motor, const double dt) const override {
		state.ticks += motor.angular_velocity / 60.0f * dt * _ticksPerRev; // Revs / Sec * Sec * Ticks / Rev
		return state;
	}
private:
	double _ticksPerRev;
};

// Encoder Mutator sets values inside of WPILib with our (now immutable) state.
class EncoderMutator : public sim::Mutator<struct EncoderState, FakeEncoderData> {
public:
	void mutate(const struct EncoderState state, FakeEncoderData &data) const override {
		data.ticks = state.ticks;
	}
};

class Robot: public IterativeRobot {
public:
	// Both of these would be arrays in reality but I'm only testing one motor for this time.
	struct MotorState motorState;
	struct EncoderState encoderState;

	MotorModel cimModel;
	RotaryEncoderModel encModel;

	EncoderMutator encMutate;

	Talon talon;
	
	double lastTime = 0;
	Robot() : cimModel(5310), encModel(1024), talon(1) { }

    void RobotInit() {
		printf("time,pwm,motor,encoder\n");
		for (double t = 0; t < 10; t += 0.01) {
			loopfunc(t, 0.01);
		}
	}

	void loopfunc(double time, double dt) {
		// This is just fake data which in reality would be provided by wpilib.
		FakeEncoderData fakeEncoder;
		// This would be part of the user code.
		talon.Set(0.25 + 0.5*sin(2 * 3.1415 * time / 2));	// Period = 2s

		// All this would be a part of another class written by the user for simulation only.
		sim::PWMData pwmData = sim::GetPWMData(1);
		motorState = cimModel.calculate(motorState, pwmData, dt);
		encoderState = encModel.calculate(encoderState, motorState, dt);
		encMutate.mutate(encoderState, fakeEncoder);

		printf("%f,%f,%f,%ld\n", time, pwmData.speed, motorState.angular_velocity, fakeEncoder.ticks);
	}
};

START_ROBOT_CLASS(Robot)