File content as of revision 2:df0c6af898ac:

// GEAR RATIO AND ENCODER COUNTS
#define GEAR_RATIO       131
#define COUNTS_PER_REV   64

// Motor 1
#define M1_DIR              D7
#define M1_PWM              D6
#define M1_ENC_A            D10
#define M1_ENC_B            D11

// Motor 2
#define M2_DIR              D4
#define M2_PWM              D5
#define M2_ENC_A            D12
#define M2_ENC_B            D13

#include "motor.h"
#include "controller.h"



/**
 * Robot
 *
 * Implementation can be found in main.cpp
 */
namespace robot
{
// sample time
const double kSampleTime = 0.001;

// Robot states
enum State {
    OFF,
    CALIBRATION,
    HOMING,
    READY,
    DEMO,
    MANUAL
};

// Robot state description
const char *StateNames[] = {
    "Off",
    "Calibration",
    "Homing",
    "Ready",
    "Demo",
    "Manual"
};

// State changing
void SwitchState(State new_state);
void GoToNextState();


/**
 * Robot properties
 */
const double kL1 = 0.30;
const double kL2 = 0.38;

const double kCalibAngleMotor1 = 140;
const double kCalibAngleMotor2 = -3;

/**
 * Motors and controllers
 */
Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV);
Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true);

Controller c1(.500,.200,0.070,kSampleTime);
Controller c2(.400,.200,0.050,kSampleTime);

/**
 * Robot state
 */
State state = OFF;
bool is_calibrated = false;

bool has_power();

double get_angle(int i);
double get_x();
double get_y();

/**
 * Forward and inverse kinematics
 */
double ForwardKinematicsX(double theta_1, double theta_2);
double ForwardKinematicsY(double theta_1, double theta_2);
double InverseKinematicsTheta(double x, double y, int i);

/**
 * Robot actions
 */
void Start();
void Stop();
void ControlLoop();

void SetCalibrationAngles();
}