Eduard Medla
Regenerating PPM signal based on distances from ultrasonic sensors, ESP8266 for connectin via wifi. Autonomous quadcopter behaviour, autonomou height holding. Flying direction based on front and back ultrasonic sensors.
Dependencies: ConfigFile HCSR04 PID PPM2 mbed-rtos mbed
Revision 41:5fe200d20022, committed 2018-05-22
- Comitter:
- edy05
- Date:
- Tue May 22 19:43:09 2018 +0000
- Branch:
- DistanceRegulation
- Parent:
- 40:0aa1cefe80ab
- Commit message:
- final updates
Changed in this revision
diff -r 0aa1cefe80ab -r 5fe200d20022 ESP8266/Server.h
--- a/ESP8266/Server.h Tue May 15 10:34:35 2018 +0000
+++ b/ESP8266/Server.h Tue May 22 19:43:09 2018 +0000
@@ -70,9 +70,6 @@
timeout=6000;
getcount=500;
getreply();
- //Thread::wait(3000);
- //reset = 1;
- //Thread::wait(3000);
esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
startserver();
webcounter = 0;
@@ -80,13 +77,10 @@
bool pageWasSent = false;
while(1){
if(DataRX==1) {
- //timer.reset();
timer.start();
ReadWebData();
// Save data to configFile
- //writeSettingsToConfig();
if ((servreq == 1 && weberror == 0) && pageWasSent == false) {
- //////pc.printf("Send page \n\r");
// send HTTP Response
sendpage();
pageWasSent = true;
@@ -113,9 +107,6 @@
if(_newP == _P && _newI == _I && _newD == _D){
_onlyDistanChanged = true;
}
- //if(_newGroundSetPoint != _groundSetPoint){
- // _onlyDistanChanged = true;
- //}
_groundSetPoint = _newGroundSetPoint;
_P = _newP;
_I = _newI;
@@ -159,54 +150,64 @@
// strcat(webbuff, "<p><input type=\"radio\" name=\"led1\" value=\"0\" > LED 1 off");
// strcat(webbuff, "<br><input type=\"radio\" name=\"led1\" value=\"1\" checked> LED 1 on<br>");
//}
- // ground regulation
- if(_tempGroundRegulation) {
- strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" checked> Ground regulation ON");
- strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" > Ground regulation OFF<br>");
- } else {
- strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" > Ground regulation ON");
- strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" checked> Ground regulation OFF<br>");
- }
// pid ground min output
- strcat(webbuff, "pid min Output: <input type=\"text\" name=\"groundPidMinOutput\" size=6 value=\"");
+ strcat(webbuff, "<table><tr>");
+ strcat(webbuff, "<td>PID min: </td> <td><input type=\"text\" name=\"groundPidMinOutput\" size=\"4\" value=\"");
ConvertToCharArray(_groundPidMinOutput);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
+ strcat(webbuff, "\">");
+ strcat(webbuff, "</td></tr>");
+ strcat(webbuff, "<tr>");
//pid ground max output
- strcat(webbuff, "pid max Output: <input type=\"text\" name=\"groundPidMaxOutput\" size=6 value=\"");
+ strcat(webbuff, "<td>PID max: </td><td> <input type=\"text\" name=\"groundPidMaxOutput\" size=\"4\" value=\"");
ConvertToCharArray(_groundPidMaxOutput);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
+ strcat(webbuff, "\">");
+ strcat(webbuff, "</td></tr>");
//ground regulation bias
- strcat(webbuff, "Bias: <input type=\"text\" name=\"bias\" size=6 value=\"");
+ strcat(webbuff, "<tr>");
+ strcat(webbuff, "<td>Bias: </td><td><input type=\"text\" name=\"bias\" size=\"4\" value=\"");
ConvertToCharArray(_bias);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
- strcat(webbuff, "P: <input type=\"text\" name=\"proportional\" size=6 value=\"");
+ strcat(webbuff, "\">");
+ strcat(webbuff, "</td></tr>");
+ strcat(webbuff, "<tr>");
+ strcat(webbuff, "<td>P:</td><td> <input type=\"text\" name=\"proportional\" size=\"4\" value=\"");
ConvertToCharArray(_newP);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
- strcat(webbuff, "I: <input type=\"text\" name=\"integral\" size=6 value=\"");
+ strcat(webbuff, "\"></td></tr>");
+ strcat(webbuff, "<tr>");
+ strcat(webbuff, "<td>I: </td><td><input type=\"text\" name=\"integral\" size=\"4\" value=\"");
ConvertToCharArray(_newI);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
- strcat(webbuff, "D: <input type=\"text\" name=\"derivative\" size=6 value=\"");
+ strcat(webbuff, "\"></td></tr>");
+ strcat(webbuff, "<tr>");
+ strcat(webbuff, "<td>D: </td><td><input type=\"text\" name=\"derivative\" size=\"4\" value=\"");
ConvertToCharArray(_newD);
strcat(webbuff, _str);
- strcat(webbuff, "\"><br>");
+ strcat(webbuff, "\"></td></tr>");
+ strcat(webbuff, "</table><br>");
//Ground set Point
- strcat(webbuff, "Ground setPoint: <input type=\"text\" name=\"groundSetPoint\" size=6 value=\"");
+ strcat(webbuff, "Regulovana vyska: <input type=\"text\" name=\"groundSetPoint\" size=\"4\" value=\"");
ConvertToCharArray(_newGroundSetPoint);
strcat(webbuff, _str);
strcat(webbuff, "\"><br>");
- if(_goAhead == false) {
- strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"0\" checked> Go ahead OFF");
- strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"1\" > Go ahead ON</br>");
+ // ground regulation
+ if(_tempGroundRegulation) {
+ strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" checked> Regulace vysky - zapnout");
+ strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" > Regulace vysky - vypnout</p>");
} else {
- strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"0\" > Go ahead OFF");
- strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"1\" checked> Go ahead ON</br>");
+ strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" > Regulace vysky - zapnout");
+ strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" checked> Regulace vysky - vypnout<p>");
}
- strcat(webbuff, "<p><input type=\"radio\" name=\"nothing\" value=\"1\" checked></p>");
+ if(_goAhead == false) {
+ strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" > Autonomni pohyb - zapnout");
+ strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\" checked> Autonomni pohyb - vypnout</p>");
+ } else {
+ strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" checked> Autonomni pohyb - zapnout");
+ strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\"> Autonomni pohyb - vypnout</p>");
+ }
+ strcat(webbuff, "<p><input type=\"radio\" name=\"nothing\" value=\"1\" style=\"display:none\" checked></p>");
strcat(webbuff, "<p><input type=\"submit\"></p>");
strcat(webbuff, "</form>");
strcat(webbuff, "</body></html>");
@@ -430,11 +431,14 @@
//////pc.printf("\n\r end of looking for data \n\r");
}
//Go ahead
- if (strstr(webdata, "goAhead=1") != NULL )
+ if (strstr(webdata, "goAhead=1") != NULL ){
+ pc.printf("go agead ON \n\r");
_goAhead = true;
- if (strstr(webdata, "goAhead=0") != NULL )
+ }
+ if (strstr(webdata, "goAhead=0") != NULL ){
+ pc.printf("go agead OFF \n\r");
_goAhead = false;
-
+ }
pc.printf("test \n\r");
sprintf(channel, "%d",linkID);
if (strstr(webdata, "GET") != NULL) {
diff -r 0aa1cefe80ab -r 5fe200d20022 HCSR04.lib --- a/HCSR04.lib Tue May 15 10:34:35 2018 +0000 +++ b/HCSR04.lib Tue May 22 19:43:09 2018 +0000 @@ -1,1 +1,1 @@ -http://os.mbed.com/users/antoniolinux/code/HCSR04/#6f6469b2f016 +https://os.mbed.com/users/edy05/code/HCSR04/#e9b99635aa2d
diff -r 0aa1cefe80ab -r 5fe200d20022 PID.lib --- a/PID.lib Tue May 15 10:34:35 2018 +0000 +++ b/PID.lib Tue May 22 19:43:09 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/aberk/code/PID/#02717c0e74ce +https://os.mbed.com/users/edy05/code/PID/#8ee2f9ba6ac3
diff -r 0aa1cefe80ab -r 5fe200d20022 PPM.lib --- a/PPM.lib Tue May 15 10:34:35 2018 +0000 +++ b/PPM.lib Tue May 22 19:43:09 2018 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/edy05/code/PPM2/#595dbe5cffdf +https://os.mbed.com/users/edy05/code/PPM2/#0c84dc8ad612
diff -r 0aa1cefe80ab -r 5fe200d20022 SRFO2/SRFO2.cpp
--- a/SRFO2/SRFO2.cpp Tue May 15 10:34:35 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,91 +0,0 @@
-#include "srf02.h"
-
-SRF02::SRF02(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr)
-{
-}
-
-SRF02::~SRF02()
-{
-}
-
-//Get the data in centimeters
-int SRF02::readcm()
-{
- char cmd[2];
- char eco_high[1],eco_low[1]; //this is because the sensor, sends the data in tow bytes, and you have tu read from two different registers 0x02 and 0x03;
-
-
- // Get range data from SRF02 in centimeters
- cmd[0] = 0x00;
- cmd[1] = 0x51;
- m_i2c.write(m_addr, cmd, 2);
-
- wait_ranging();
-
-
- cmd[0] = 0x02;
- m_i2c.write(m_addr, cmd, 1, 1);
- m_i2c.read(m_addr, eco_high, 1);
-
- cmd[0] = 0x03;
- m_i2c.write(m_addr,cmd,1,1);
- m_i2c.read(m_addr,eco_low,1);
-
- int range = (eco_high[0]<<8)|eco_low[0];
-
- return range;
-}
-
-//Get the data in inches
-int SRF02::readinch()
-{
- char cmd[2];
- char eco_high[1],eco_low[1]; //this is because the sensor, sends the data in tow bytes, and you have tu read from two different registers 0x02 and 0x03;
-
- cmd[0] = 0x00;
- cmd[1] = 0x50;
- m_i2c.write(m_addr, cmd, 2);
-
- wait_ranging();
-
- cmd[0] = 0x02;
- m_i2c.write(m_addr, cmd, 1, 1);
- m_i2c.read(m_addr, eco_high, 1);
-
- cmd[0] = 0x03;
- m_i2c.write(m_addr,cmd,1,1);
- m_i2c.read(m_addr,eco_low,1);
-
- int range = (eco_high[0]<<8)|eco_low[0];
-
- return range;
-}
-
-//Change adress of the device. Remember to have only one sensor conected to execute this method.
-void SRF02::change_addr(char new_addr)
-{
- char cmd[2];
-
- cmd[0]=0x00;
- cmd[1]=0xA0;
- m_i2c.write(m_addr,cmd,2);
- cmd[0]=0x00;
- cmd[1]=0xAA;
- m_i2c.write(m_addr,cmd,2);
- cmd[0]=0x00;
- cmd[1]=0xA5;
- m_i2c.write(m_addr,cmd,2);
- cmd[0]=0x00;
- cmd[1]=new_addr;
- m_i2c.write(m_addr,cmd,2);
-
- }
-
-void SRF02::wait_ranging(void)
-{
- char eco;
-
- do {
- eco=m_i2c.read(1);
- }while(eco == 0xff);
-}
\ No newline at end of file
diff -r 0aa1cefe80ab -r 5fe200d20022 SRFO2/srf02.h
--- a/SRFO2/srf02.h Tue May 15 10:34:35 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,42 +0,0 @@
-#ifndef SRF02_H
-#define SRF02_H
-
-#include "mbed.h"
-
-
-/** Library to control SRF02 ultrasonic sensor */
-class SRF02
-{
- public:
- /** Creates an instance of the class
- *
- * @param sda I2C sda Pin
- * @param scl I2C scl Pin
- */
- SRF02(PinName sda, PinName scl, int addr);
-
- /** Destroys instance */
- ~SRF02();
-
- /** Read the range data in centimeters */
- int readcm();
-
- /** Read the range data in inches */
- int readinch();
-
- /** Change the adress of the device. This is very usefull when there are more sensors.
- * This function must be executed with only one sensor conected.
- */
- void change_addr(char new_addr);
-
- private:
- /** wait for ranging to complete
- * This function is for internal use
- */
- void wait_ranging(void);
- I2C m_i2c;
- int m_addr;
-
-};
-
-#endif
\ No newline at end of file
diff -r 0aa1cefe80ab -r 5fe200d20022 definitions.h --- a/definitions.h Tue May 15 10:34:35 2018 +0000 +++ b/definitions.h Tue May 22 19:43:09 2018 +0000 @@ -16,9 +16,10 @@ #define AUX1 4 #define AUX2 5 -#define THROTTLE_LIMIT 1500 -#define PITCH_GO_AHEAD 1512 -#define PITCH_GO_BACK 1488 +#define THROTTLE_LIMIT 1500 +#define PITCH_MOVE_FORWARD 15 +#define PITCH_MOVE_BACKWARD 15 +#define LANDING_HEIGHT 6 #define FLIGHT_CONTROLLER_FREQUENCY 100
diff -r 0aa1cefe80ab -r 5fe200d20022 distanceRegulation.h
--- a/distanceRegulation.h Tue May 15 10:34:35 2018 +0000
+++ b/distanceRegulation.h Tue May 22 19:43:09 2018 +0000
@@ -11,18 +11,24 @@
pc.printf("Flight controller thread started\r\n");
- _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);
+ _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void*)0);
int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
_groudRegulationUpdateTimer->start(rate);
+ osThreadId id;
+ id = Thread::gettid();
+ pc.printf("regulation gettid 0x%08X \n\r", id);
+
Thread::wait(osWaitForever);
}
void distanceRegulationTask(void const *args){
- //pc.printf("Flight controller task started\r\n");
-
+ //osThreadId id;
+ //id = Thread::gettid();
+ //pc.printf("regulation task gettid 0x%08X \n\r", id);
+
uint16_t channels[CHANNELS];
float distance1=0;
float groundDistancePIDValue;
@@ -44,98 +50,86 @@
pc.printf("setpoint %f \n\r", _groundSetPoint);
pc.printf("bias %f \n\r", _bias);
_groundDistance->resetError();
- //pc.printf("PID tunings changed \n\r");
_groundDistance->setTunings(_P, _I, _D);
_groundDistance->setSetPoint(_groundSetPoint);
_groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
- //_groundDistance->setBias(_bias);
}
}
//distance1 = _sonic->getDistan1();
- //pc.printf("%f \n\r", distance1);
+ //pc.printf(" distance1 %f \n\r", distance1);
if(_groundRegulation){
- //timer.reset();
- //timer.start();
-
+ distance1 = _sonic->getDistan1();
_ppmRegen->getAllChannels(channels);
+ _groundDistance->setProcessValue(distance1);
- distance1 = _sonic->getDistan1();
- //distance2 = _sonic->getDistan2();
- //pc.printf("%f \n\r", distance1);
- //float p = _groundDistance->getPParam();
- //float i = _groundDistance->getIParam();
- //float d = _groundDistance->getDParam();
- //float max = _groundDistance->getInMax();
- //pc.printf("%f \n\r", max);
- //pc.printf("%f \n\r", p);
- //pc.printf("%f \n\r", i);
- //pc.printf("%f \n\r", d);
- //pc.printf("distance2: %d \n\r", distance2);
- _groundDistance->setProcessValue(distance1);
+ //landing
+ if(channels[AUX1] >= 1300){
+ //pc.printf("landing \n\r");
+ if(distance1 <= LANDING_HEIGHT)
+ channels[AUX2] = 1000;
+ _groundDistance->setLandingPoint((float)LANDING_HEIGHT);
+ }
+
groundDistancePIDValue = _groundDistance->compute();
- //pc.printf("pid value %f \n\r", groundDistancePIDValue);
//Update PWM values
regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
if(channels[AUX2] < 1500){
channels[THROTTLE] = 1010;
}
+
+ // limits of throttle
if(regulatedThrottle > THROTTLE_LIMIT)
regulatedThrottle = THROTTLE_LIMIT;
if(regulatedThrottle < 1010)
regulatedThrottle = 1010;
-
- //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
- //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
- //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
-
- // goAhead for ROLL
- if(_goAhead == true && _groundDistance->quadStabilized() && _frontDistance >= 40){
- _pitch->pulsewidth_us( PITCH_GO_AHEAD);
- }
- if(_frontDistance < 40){
- _goAhead = false;
- _frontWall = true;
- _pitch->pulsewidth_us( PITCH_GO_BACK);
+ // goAhead and back
+ if(_groundDistance->quadStabilized() && _goAhead){
+ //pc.printf("going agead \n\r");
+ if(_frontDistance <= 40){
+ _frontWall = true;
+ _backWall = false;
+ }
+ if(_backDistance <= 40){
+ _frontWall = false;
+ _backWall = true;
+ }
+ if(_frontWall){
+ //pc.printf(" going backward \n\r");
+ _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD);
+ }
+ if(_backWall){
+ //pc.printf(" going forward \n\r");
+ _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD);
+ }
+
+
+ //if(_frontDistance > 40 && _backDistance > 40)
+ // _pitch->pulsewidth_us(channels[PITCH]);
+
}
else{
- _pitch->pulsewidth_us( channels[PITCH]);
+ _pitch->pulsewidth_us(channels[PITCH]);
}
- //pc.printf("front distance %d \n\r", _frontDistance);
-
- //if(_frontWall){
- //
- //}
-
-
-
_roll->pulsewidth_us( channels[ROLL]);
_yaw->pulsewidth_us( channels[YAW]);
_throttle->pulsewidth_us(regulatedThrottle);
_aux1->pulsewidth_us( channels[AUX1]);
_aux2->pulsewidth_us( channels[AUX2]);
- //timer.stop();
- //pc.printf("Timer: %d \n\r", timer.read_us());
}
+
else {
- //distance = _sonic->getDistan();
- //pc.printf("Sonic distance: %d \n\r", distance);
- //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
+ if(_groundDistance->quadStabilized())
+ _groundDistance->setNotStabelized();
- //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
+
// Generate PWM
_ppmRegen->getAllChannels(channels);
- //pc.printf("channel value: %d \n\r", channels[ROLL]);
- //pc.printf("channel value: %d \n\r", channels[PITCH]);
- //pc.printf("channel value: %d \n\r", channels[YAW]);
- //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
- //pc.printf("channel value: %d \n\r", channels[AUX1]);
- //pc.printf("channel value: %d \n\r", channels[AUX2]);
_roll->pulsewidth_us( channels[ROLL]);
_pitch->pulsewidth_us( channels[PITCH]);
@@ -145,4 +139,5 @@
_aux2->pulsewidth_us( channels[AUX2]);
}
+
}
\ No newline at end of file
diff -r 0aa1cefe80ab -r 5fe200d20022 hardware.h
--- a/hardware.h Tue May 15 10:34:35 2018 +0000
+++ b/hardware.h Tue May 22 19:43:09 2018 +0000
@@ -36,7 +36,7 @@
Thread *_distanceThread;
-InterruptIn* _interruptPort = new InterruptIn(p28);
+InterruptIn* _interruptPin = new InterruptIn(p28);
PwmOut* _roll = new PwmOut(p21);
PwmOut* _pitch = new PwmOut(p22);
PwmOut* _throttle = new PwmOut(p23);
@@ -53,18 +53,19 @@
float _P = 0;
float _I = 0;
float _D = 0;
-float _groundSetPoint = 0;
-float _bias = 0;
-float _groundPidMinOutput = 0;
-float _groundPidMaxOutput = 0;
+int _groundSetPoint = 0;
+int _bias = 0;
+int _groundPidMinOutput = 0;
+int _groundPidMaxOutput = 0;
bool _groundRegulation = false;
// Temporary values for Server to filter noise
float _newP;
float _newI;
float _newD;
-float _newGroundSetPoint;
+int _newGroundSetPoint;
bool _tempGroundRegulation = false;
bool _goAhead = false;
+bool _backWall = true;
bool _frontWall = false;
@@ -72,6 +73,7 @@
+
void loadConfigFile(void){
//reading configFile
_configFile.read("/local/config.cfg");
@@ -133,7 +135,7 @@
//Converts float to char array
void ConvertToCharArray(float number)
{
- sprintf(_str, "%3.6f", number );
+ sprintf(_str, "%3.2f", number );
}
//Converts integer to char array
diff -r 0aa1cefe80ab -r 5fe200d20022 main.cpp
--- a/main.cpp Tue May 15 10:34:35 2018 +0000
+++ b/main.cpp Tue May 22 19:43:09 2018 +0000
@@ -5,7 +5,7 @@
#include "PpmRegen.h"
#include "distanceRegulation.h"
#include "hcsr04.h"
-#include "front_sensor.h"
+#include "front_back_sensors.h"
#include "PID.h"
#include "ConfigFile.h"
@@ -14,19 +14,21 @@
//VARIABLES
uint16_t ppmInChannelsValue[CHANNELS];
-volatile uint16_t distance = 0;
+
//FUNCTIONS
void print_ppmIn(void);
-//RtosTimer distanceRegulation(distanceRegulationTask, osTimerPeriodic, (void *)0);
int main(){
-
pc.baud(115200);
+ osThreadId mainID;
+ mainID = Thread::gettid();
+ pc.printf("main gettid 0x%08X \n\r", mainID);
+
// INITIALIZE CLASSES
- _ppmRegen = new PpmRegen(_interruptPort);
+ _ppmRegen = new PpmRegen(_interruptPin);
float rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY);
_groundDistance = new PID(0, 0, 0, rate);
@@ -46,6 +48,8 @@
// STARTING THREADS
pc.printf("starting distance thread \n\r");
_distanceThread = new Thread(distanceRegulationThread);
+ osThreadId distanceID;
+ pc.printf("distance gettid 0x%08X \n\r", distanceID);
_distanceThread->set_priority(osPriorityRealtime);
//_sonic = new HCSR04(p7, p8, p5, p6);
@@ -57,19 +61,18 @@
//_leftSensorThread.start(left_sensor);
//_leftSensorThread.set_priority(osPriorityHigh);
- _leftSensorThread.start(callback(semaphore_thread, (void *)"leftSensor"));
+ pc.printf("Starting sonic threads \n\r");
_frontSensorThread.start(callback(semaphore_thread, (void *)"frontSensor"));
- _rightSensorThread.start(callback(semaphore_thread, (void *)"rightSensor"));
_backSensorThread.start(callback(semaphore_thread, (void *)"backSensor"));
_frontSensorThread.set_priority(osPriorityHigh);
- _leftSensorThread.set_priority(osPriorityHigh);
- _rightSensorThread.set_priority(osPriorityHigh);
_backSensorThread.set_priority(osPriorityHigh);
pc.printf("starting server thread \n\r");
+ osThreadId serverID;
+ pc.printf("server gettid 0x%08X \n\r", serverID);
_serverThread.start(serverRun);
_serverThread.set_priority(osPriorityHigh);
@@ -78,6 +81,7 @@
//wait(1);
+
while(1){
Thread::wait(osWaitForever);
}