Eduard Medla / Mbed 2 deprecated Autonomous_quadcopter

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

distanceRegulation.h@22:db086d070cdf, 2017-10-29 (annotated)

Committer:
edy05
Date:
Sun Oct 29 14:43:16 2017 +0000
Branch:
DistanceRegulation
Revision:
22:db086d070cdf
Parent:
19:9ceec179b140
Child:
23:46ae5b0464da
distanceRegulation - added trigger

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edy05 2:d172c9963f87 1 #include "mbed.h"
edy05 2:d172c9963f87 2 #include "rtos.h"
edy05 2:d172c9963f87 3 #include "hardware.h"
edy05 2:d172c9963f87 4 #include "definitions.h"
edy05 2:d172c9963f87 5
edy05 2:d172c9963f87 6 void distanceRegulationThread(){
edy05 2:d172c9963f87 7 uint16_t channels[CHANNELS];
edy05 2:d172c9963f87 8 int distance=0;
edy05 18:e0c5ab76f337 9 float groundDistancePIDValue;
edy05 19:9ceec179b140 10 int regulatedThrottle;
edy05 2:d172c9963f87 11
edy05 2:d172c9963f87 12 //setup PID
edy05 2:d172c9963f87 13 _groundDistance->setInputLimits(0, 500);
edy05 22:db086d070cdf 14 _groundDistance->setOutputLimits(-400, 400);
edy05 2:d172c9963f87 15 _groundDistance->setMode(AUTO_MODE);
edy05 2:d172c9963f87 16 _groundDistance->setSetPoint(0.0);
edy05 2:d172c9963f87 17 _groundDistance->setBias(0);
edy05 13:33024b5880b3 18 _groundDistance->setTunings(_P, _I, _D);
edy05 2:d172c9963f87 19
edy05 2:d172c9963f87 20 while(1){
edy05 22:db086d070cdf 21 if(_groundRegulation){
edy05 22:db086d070cdf 22 //if config has changed
edy05 22:db086d070cdf 23 if(_configChanges){
edy05 22:db086d070cdf 24 _configChanges = false;
edy05 22:db086d070cdf 25 pc.printf("PID tunings changed \n\r");
edy05 22:db086d070cdf 26 _groundDistance->setTunings(_P, _I, _D);
edy05 22:db086d070cdf 27 _groundDistance->setSetPoint(_groundSetPoint);
edy05 22:db086d070cdf 28 }
edy05 22:db086d070cdf 29 distance = _sonic->getDistan();
edy05 22:db086d070cdf 30 _groundDistance->setProcessValue(distance);
edy05 22:db086d070cdf 31 groundDistancePIDValue = _groundDistance->compute();
edy05 22:db086d070cdf 32
edy05 22:db086d070cdf 33 //Update PWM values
edy05 22:db086d070cdf 34 _ppmRegen->getAllChannels(channels);
edy05 22:db086d070cdf 35 regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
edy05 22:db086d070cdf 36 if(regulatedThrottle > 2000)
edy05 22:db086d070cdf 37 regulatedThrottle = 2000;
edy05 22:db086d070cdf 38 else if(regulatedThrottle < 1100)
edy05 22:db086d070cdf 39 regulatedThrottle = 1100;
edy05 22:db086d070cdf 40
edy05 22:db086d070cdf 41
edy05 22:db086d070cdf 42 pc.printf("Sonic distance: %d \n\r", distance);
edy05 22:db086d070cdf 43 pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 22:db086d070cdf 44 pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 22:db086d070cdf 45 pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 22:db086d070cdf 46
edy05 22:db086d070cdf 47 // Generate regulated PWM
edy05 22:db086d070cdf 48 _roll->pulsewidth_us( channels[ROLL]);
edy05 22:db086d070cdf 49 _pitch->pulsewidth_us( channels[PITCH]);
edy05 22:db086d070cdf 50 _yaw->pulsewidth_us( channels[YAW]);
edy05 22:db086d070cdf 51 _throttle->pulsewidth_us(regulatedThrottle);
edy05 22:db086d070cdf 52 _aux1->pulsewidth_us( channels[AUX1]);
edy05 22:db086d070cdf 53 _aux2->pulsewidth_us( channels[AUX2]);
edy05 13:33024b5880b3 54 }
edy05 22:db086d070cdf 55 else {
edy05 22:db086d070cdf 56
edy05 22:db086d070cdf 57 // Generate PWM
edy05 22:db086d070cdf 58 _ppmRegen->getAllChannels(channels);
edy05 22:db086d070cdf 59 _roll->pulsewidth_us( channels[ROLL]);
edy05 22:db086d070cdf 60 _pitch->pulsewidth_us( channels[PITCH]);
edy05 22:db086d070cdf 61 _yaw->pulsewidth_us( channels[YAW]);
edy05 22:db086d070cdf 62 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 22:db086d070cdf 63 _aux1->pulsewidth_us( channels[AUX1]);
edy05 22:db086d070cdf 64 _aux2->pulsewidth_us( channels[AUX2]);
edy05 22:db086d070cdf 65 }
edy05 2:d172c9963f87 66
edy05 2:d172c9963f87 67 Thread::wait(500);
edy05 2:d172c9963f87 68
edy05 2:d172c9963f87 69
edy05 2:d172c9963f87 70 }
edy05 2:d172c9963f87 71
edy05 2:d172c9963f87 72 }