Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

main.cpp

Committer:
aolgu003
Date:
2016-07-29
Revision:
7:396fa2a8648d
Parent:
6:b45b74fd6a07

File content as of revision 7:396fa2a8648d:

// Continuously sweep the servo through it's full range
#include "mbed.h"
#include "vessel.h"
//#include "MS5803.h"
//MS5803 big_sensor = MS5803(I2C_SDA, I2C_SCL, ms5803_addrCH);

int main()
{  
    Timer t;
    wait(3);
    Vessel seagoat; //Starts the seagoat
    seagoat.SetYawPID(1,0,0);  
    seagoat.SetRollPID(1,0,0);    
    seagoat.SetPitchPID(1,0,0);    
    seagoat.SetXPID(0,0,0);    
    seagoat.SetYPID(0,0,0);    
    seagoat.SetZPID(0,0,0);    
    
    t.start();
    while(t.read() < 4){
         seagoat.calibrate();
         
         //pc.printf("%f \n", t.read());
    }
    t.stop();
    pc.printf("Seagoat Ready to Go\n\r");
    //seagoat.motorTest();
    float depth = 0;
    while(1) {
        //seagoat.motorTest();
        seagoat.update();
        seagoat.updateCommand();
        wait(0.01);
    }
}