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@13:33024b5880b3, 2017-10-27 (annotated)
- Committer:
- edy05
- Date:
- Fri Oct 27 10:47:12 2017 +0000
- Revision:
- 13:33024b5880b3
- Parent:
- 3:6c9f80f5e865
- Child:
- 16:9490bb68e1fb
merge
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:d172c9963f87 | 9 | float groundDistancePidValue; |
edy05 | 2:d172c9963f87 | 10 | int newThrottleValue; |
edy05 | 2:d172c9963f87 | 11 | |
edy05 | 2:d172c9963f87 | 12 | //setup PID |
edy05 | 2:d172c9963f87 | 13 | _groundDistance->setInputLimits(0, 500); |
edy05 | 2:d172c9963f87 | 14 | _groundDistance->setOutputLimits(-100, 100); |
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 | 2:d172c9963f87 | 21 | |
edy05 | 2:d172c9963f87 | 22 | _ppmRegen->getAllChannels(channels); |
edy05 | 2:d172c9963f87 | 23 | distance = _sonic->getDistan(); |
edy05 | 2:d172c9963f87 | 24 | |
edy05 | 2:d172c9963f87 | 25 | //calculate PID for given channel |
edy05 | 13:33024b5880b3 | 26 | if(pidChanged){ |
edy05 | 13:33024b5880b3 | 27 | pidChanged = false; |
edy05 | 13:33024b5880b3 | 28 | pc.printf("PID tunings changed \n\r"); |
edy05 | 13:33024b5880b3 | 29 | _groundDistance->setTunings(_P, _I, _D); |
edy05 | 13:33024b5880b3 | 30 | } |
edy05 | 2:d172c9963f87 | 31 | _groundDistance->setProcessValue(distance); |
edy05 | 2:d172c9963f87 | 32 | _groundDistance->setSetPoint(30); |
edy05 | 2:d172c9963f87 | 33 | groundDistancePidValue = _groundDistance->compute(); |
edy05 | 13:33024b5880b3 | 34 | |
edy05 | 2:d172c9963f87 | 35 | //Update PWM values |
edy05 | 2:d172c9963f87 | 36 | newThrottleValue = channels[THROTTLE] + groundDistancePidValue; |
edy05 | 2:d172c9963f87 | 37 | |
edy05 | 2:d172c9963f87 | 38 | pc.printf("Sonic distance: %d \n\r", distance); |
edy05 | 2:d172c9963f87 | 39 | pc.printf("channel throttle pid value: %f \n\r", groundDistancePidValue); |
edy05 | 2:d172c9963f87 | 40 | pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]); |
edy05 | 2:d172c9963f87 | 41 | pc.printf("channel throttle final value: %d \n\r", newThrottleValue); |
edy05 | 2:d172c9963f87 | 42 | |
edy05 | 2:d172c9963f87 | 43 | // Generate PWM |
edy05 | 2:d172c9963f87 | 44 | _roll->pulsewidth_us( channels[ROLL]); |
edy05 | 2:d172c9963f87 | 45 | _pitch->pulsewidth_us( channels[PITCH]); |
edy05 | 2:d172c9963f87 | 46 | _yaw->pulsewidth_us( channels[YAW]); |
edy05 | 2:d172c9963f87 | 47 | _throttle->pulsewidth_us(channels[THROTTLE]); |
edy05 | 2:d172c9963f87 | 48 | _aux1->pulsewidth_us( channels[AUX1]); |
edy05 | 2:d172c9963f87 | 49 | _aux2->pulsewidth_us( channels[AUX2]); |
edy05 | 2:d172c9963f87 | 50 | |
edy05 | 2:d172c9963f87 | 51 | Thread::wait(500); |
edy05 | 2:d172c9963f87 | 52 | |
edy05 | 2:d172c9963f87 | 53 | |
edy05 | 2:d172c9963f87 | 54 | } |
edy05 | 2:d172c9963f87 | 55 | |
edy05 | 2:d172c9963f87 | 56 | } |