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@26:11539036f0fb, 2017-12-01 (annotated)

Committer:
edy05
Date:
Fri Dec 01 11:11:23 2017 +0000
Branch:
DistanceRegulation
Revision:
26:11539036f0fb
Parent:
25:69190c222dbf
Child:
27:5956d5e3ff63
Stable

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 26:11539036f0fb 6
edy05 26:11539036f0fb 7 //RtosTimer *_groudRegulationUpdateTimer;
edy05 26:11539036f0fb 8
edy05 26:11539036f0fb 9 /*
edy05 26:11539036f0fb 10 void distanceRegulationThread(void const *args){
edy05 26:11539036f0fb 11
edy05 26:11539036f0fb 12 _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);
edy05 26:11539036f0fb 13 int rate = 2;
edy05 26:11539036f0fb 14 _groudRegulationUpdateTimer->start(rate);
edy05 26:11539036f0fb 15
edy05 26:11539036f0fb 16 Thread::wait(osWaitForever);
edy05 26:11539036f0fb 17
edy05 26:11539036f0fb 18 }
edy05 26:11539036f0fb 19 */
edy05 26:11539036f0fb 20 void distanceRegulationTask(){
edy05 2:d172c9963f87 21 uint16_t channels[CHANNELS];
edy05 2:d172c9963f87 22 int distance=0;
edy05 18:e0c5ab76f337 23 float groundDistancePIDValue;
edy05 19:9ceec179b140 24 int regulatedThrottle;
edy05 2:d172c9963f87 25
edy05 2:d172c9963f87 26 //setup PID
edy05 26:11539036f0fb 27 _groundDistance->setInputLimits(0, 300);
edy05 25:69190c222dbf 28 //_groundDistance->setOutputLimits(100, 300);
edy05 2:d172c9963f87 29 _groundDistance->setMode(AUTO_MODE);
edy05 25:69190c222dbf 30 //_groundDistance->setSetPoint(0.0);
edy05 25:69190c222dbf 31 //_groundDistance->setBias(0);
edy05 13:33024b5880b3 32 _groundDistance->setTunings(_P, _I, _D);
edy05 2:d172c9963f87 33
edy05 26:11539036f0fb 34 //Timer timer;
edy05 26:11539036f0fb 35
edy05 2:d172c9963f87 36 while(1){
edy05 22:db086d070cdf 37 if(_groundRegulation){
edy05 26:11539036f0fb 38 //timer.reset();
edy05 26:11539036f0fb 39 //timer.start();
edy05 26:11539036f0fb 40
edy05 25:69190c222dbf 41 _ppmRegen->getAllChannels(channels);
edy05 25:69190c222dbf 42 //PID SETUP
edy05 22:db086d070cdf 43 if(_configChanges){
edy05 22:db086d070cdf 44 _configChanges = false;
edy05 26:11539036f0fb 45 _groundDistance->resetError();
edy05 23:46ae5b0464da 46 //pc.printf("PID tunings changed \n\r");
edy05 22:db086d070cdf 47 _groundDistance->setTunings(_P, _I, _D);
edy05 22:db086d070cdf 48 _groundDistance->setSetPoint(_groundSetPoint);
edy05 25:69190c222dbf 49 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 26:11539036f0fb 50 _groundDistance->setBias(_bias);
edy05 22:db086d070cdf 51 }
edy05 22:db086d070cdf 52 distance = _sonic->getDistan();
edy05 22:db086d070cdf 53 _groundDistance->setProcessValue(distance);
edy05 22:db086d070cdf 54 groundDistancePIDValue = _groundDistance->compute();
edy05 25:69190c222dbf 55 groundDistancePIDValue += _bias;
edy05 25:69190c222dbf 56
edy05 22:db086d070cdf 57
edy05 22:db086d070cdf 58 //Update PWM values
edy05 22:db086d070cdf 59 regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
edy05 25:69190c222dbf 60 if(channels[AUX2] < 1500){
edy05 25:69190c222dbf 61 channels[THROTTLE] = 1010;
edy05 25:69190c222dbf 62 }
edy05 22:db086d070cdf 63 if(regulatedThrottle > 2000)
edy05 22:db086d070cdf 64 regulatedThrottle = 2000;
edy05 25:69190c222dbf 65 if(regulatedThrottle < 1010)
edy05 25:69190c222dbf 66 regulatedThrottle = 1010;
edy05 22:db086d070cdf 67
edy05 22:db086d070cdf 68
edy05 22:db086d070cdf 69 pc.printf("Sonic distance: %d \n\r", distance);
edy05 25:69190c222dbf 70 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 25:69190c222dbf 71 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 25:69190c222dbf 72 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 22:db086d070cdf 73
edy05 22:db086d070cdf 74 // Generate regulated PWM
edy05 22:db086d070cdf 75 _roll->pulsewidth_us( channels[ROLL]);
edy05 22:db086d070cdf 76 _pitch->pulsewidth_us( channels[PITCH]);
edy05 22:db086d070cdf 77 _yaw->pulsewidth_us( channels[YAW]);
edy05 22:db086d070cdf 78 _throttle->pulsewidth_us(regulatedThrottle);
edy05 22:db086d070cdf 79 _aux1->pulsewidth_us( channels[AUX1]);
edy05 22:db086d070cdf 80 _aux2->pulsewidth_us( channels[AUX2]);
edy05 26:11539036f0fb 81 //timer.stop();
edy05 26:11539036f0fb 82 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 26:11539036f0fb 83
edy05 13:33024b5880b3 84 }
edy05 22:db086d070cdf 85 else {
edy05 22:db086d070cdf 86
edy05 25:69190c222dbf 87 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 25:69190c222dbf 88 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 25:69190c222dbf 89 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 25:69190c222dbf 90 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 22:db086d070cdf 91 // Generate PWM
edy05 22:db086d070cdf 92 _ppmRegen->getAllChannels(channels);
edy05 22:db086d070cdf 93 _roll->pulsewidth_us( channels[ROLL]);
edy05 22:db086d070cdf 94 _pitch->pulsewidth_us( channels[PITCH]);
edy05 22:db086d070cdf 95 _yaw->pulsewidth_us( channels[YAW]);
edy05 22:db086d070cdf 96 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 22:db086d070cdf 97 _aux1->pulsewidth_us( channels[AUX1]);
edy05 22:db086d070cdf 98 _aux2->pulsewidth_us( channels[AUX2]);
edy05 22:db086d070cdf 99 }
edy05 2:d172c9963f87 100
edy05 26:11539036f0fb 101 Thread::wait(2);
edy05 2:d172c9963f87 102
edy05 2:d172c9963f87 103
edy05 2:d172c9963f87 104 }
edy05 2:d172c9963f87 105
edy05 2:d172c9963f87 106 }