main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "hardware.h"
00004 #include "Server.h"
00005 #include "PpmRegen.h"
00006 #include "distanceRegulation.h"
00007 #include "hcsr04.h"
00008 #include "front_back_sensors.h"
00009 #include "PID.h"
00010 #include "ConfigFile.h"
00011 
00012 //TEMP
00013 
00014 
00015 //VARIABLES
00016 uint16_t ppmInChannelsValue[CHANNELS];
00017 
00018 
00019 //FUNCTIONS
00020 void print_ppmIn(void);
00021 
00022 
00023 int main(){
00024     pc.baud(115200);
00025     
00026     osThreadId mainID;
00027     mainID = Thread::gettid();
00028     pc.printf("main gettid 0x%08X \n\r", mainID);
00029     
00030     // INITIALIZE CLASSES
00031     _ppmRegen = new PpmRegen(_interruptPin);
00032     float rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY);
00033     _groundDistance = new PID(0, 0, 0, rate);
00034     
00035     //setup PID
00036     loadConfigFile();
00037     _newP = _P;
00038     _newI = _I;
00039     _newD = _D;
00040     _newGroundSetPoint = _groundSetPoint;
00041     // to show values in web browser after first load
00042     _groundDistance->setInputLimits(0, 300);
00043     _groundDistance->setMode(AUTO_MODE);
00044     _groundDistance->setTunings(_P, _I, _D);
00045     _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
00046     _groundDistance->setBias(_bias);
00047 
00048     // STARTING THREADS
00049     pc.printf("starting distance thread \n\r");
00050     _distanceThread = new Thread(distanceRegulationThread);
00051     osThreadId distanceID;
00052     pc.printf("distance gettid 0x%08X \n\r", distanceID);
00053     _distanceThread->set_priority(osPriorityRealtime);
00054     
00055     //_sonic = new HCSR04(p7, p8, p5, p6);
00056     _sonic = new HCSR04(p5, p6);
00057     
00058     //_frontSensorThread.start(front_sensor);
00059     //_frontSensorThread.set_priority(osPriorityHigh);
00060     
00061     //_leftSensorThread.start(left_sensor);
00062     //_leftSensorThread.set_priority(osPriorityHigh);
00063     
00064     pc.printf("Starting sonic threads \n\r");
00065     _frontSensorThread.start(callback(semaphore_thread, (void *)"frontSensor"));
00066     _backSensorThread.start(callback(semaphore_thread, (void *)"backSensor"));
00067     _frontSensorThread.set_priority(osPriorityHigh);
00068     _backSensorThread.set_priority(osPriorityHigh);
00069     
00070     
00071     
00072     
00073     pc.printf("starting server thread \n\r");
00074     osThreadId serverID;
00075     pc.printf("server gettid 0x%08X \n\r", serverID);
00076     _serverThread.start(serverRun);
00077     _serverThread.set_priority(osPriorityHigh);
00078     
00079     //distanceRegulation.start(2);
00080     //_distanceThread.start(distanceRegulationTask);
00081     
00082     //wait(1);
00083     
00084     
00085     while(1){
00086         Thread::wait(osWaitForever);
00087     }
00088         
00089 }
00090 
00091 
00092 void print_ppmIn(){
00093     _ppmRegen->getAllChannels(ppmInChannelsValue);
00094     for(uint8_t channel= 0; channel < CHANNELS; channel++){
00095         pc.printf("PPM In Channel %d: %d \n\r", channel, ppmInChannelsValue[channel]);
00096     }
00097     pc.printf("\n\r");
00098 }