File content as of revision 0:7a97ebb833eb:

#include <string>

#include "globals.h"
#include "main_controller.h"
#include "pid_controller.h"

MainController::MainController() {
    m_pid = new PIDController;
    this->stop();
}

MainController::~MainController() {
    delete m_pid;
}

void MainController::start() {
    m_pid->reset();
    shouldUpdate = true;
}

void MainController::stop() {
    shouldUpdate = false;
    m_pid->reset();
    motors.stop();
}

void MainController::update() {
    if (shouldUpdate) {
        m_pid->update();
    }
}

void MainController::driveStraight() {
    pc.printf("Driving straight.\n");
    m_pid->reset();
    
    int start = millis;
    
    m_pid->setXGoal(100);
    m_pid->setWGoal(0);

    while (encoders.right() < 5000) {
        pc.printf(m_pid->getData());
        wait(0.5);
    }
    
    int end = millis;
    
    systick.stop();
    motors.stop();
    
    pc.printf("Done moving straight: %d ms\n", end - start);
    encoders.printValues();
}

void MainController::turn(int deg) {
    pc.printf("Turning %d degrees\n", deg);
    this->start();
    
    int counts = deg * deg_to_counts;
    
    m_pid->setXGoal(0);
    m_pid->setWGoal(counts);
    
    while (!m_pid->isDone()) {
        pc.printf(m_pid->getData());
    }

    pc.printf(m_pid->getData());
    this->stop();
    pc.printf("Done turning %d degrees\n", deg);
}

void MainController::moveCells(float n) {
    pc.printf("Moving %0.2f cells\n", n);
    this->start();
    
    int counts = n * cells_to_counts;
 
    pc.printf("Set goal x counts to %d\n", counts);
    
    int start = millis;
    
    m_pid->setXGoal(counts);
    m_pid->setWGoal(0);
 
    while (!m_pid->isDone()) {
        pc.printf(m_pid->getData());
    }
 
    int end = millis;
 
    pc.printf(m_pid->getData());

    this->stop();
    pc.printf("Finished moving %0.2f cells: %d ms\n", n, end - start);
}