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
- Committer:
- edy05
- Date:
- 2017-11-05
- Branch:
- DistanceRegulation
- Revision:
- 23:46ae5b0464da
- Parent:
- 22:db086d070cdf
- Child:
- 25:69190c222dbf
File content as of revision 23:46ae5b0464da:
#include "mbed.h" #include "rtos.h" #include "hardware.h" #include "definitions.h" void distanceRegulationThread(){ uint16_t channels[CHANNELS]; int distance=0; float groundDistancePIDValue; int regulatedThrottle; //setup PID _groundDistance->setInputLimits(0, 500); _groundDistance->setOutputLimits(-600, 600); _groundDistance->setMode(AUTO_MODE); _groundDistance->setSetPoint(0.0); _groundDistance->setBias(0); _groundDistance->setTunings(_P, _I, _D); while(1){ if(_groundRegulation){ //if config has changed if(_configChanges){ _configChanges = false; //pc.printf("PID tunings changed \n\r"); _groundDistance->setTunings(_P, _I, _D); _groundDistance->setSetPoint(_groundSetPoint); } distance = _sonic->getDistan(); _groundDistance->setProcessValue(distance); groundDistancePIDValue = _groundDistance->compute(); //Update PWM values _ppmRegen->getAllChannels(channels); regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue; if(regulatedThrottle > 2000) regulatedThrottle = 2000; else if(regulatedThrottle < 1100) regulatedThrottle = 1100; pc.printf("Sonic distance: %d \n\r", distance); 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); // Generate regulated PWM _roll->pulsewidth_us( channels[ROLL]); _pitch->pulsewidth_us( channels[PITCH]); _yaw->pulsewidth_us( channels[YAW]); _throttle->pulsewidth_us(regulatedThrottle); _aux1->pulsewidth_us( channels[AUX1]); _aux2->pulsewidth_us( channels[AUX2]); } else { pc.printf("Sonic distance: %d \n\r", distance); 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); // Generate PWM _ppmRegen->getAllChannels(channels); _roll->pulsewidth_us( channels[ROLL]); _pitch->pulsewidth_us( channels[PITCH]); _yaw->pulsewidth_us( channels[YAW]); _throttle->pulsewidth_us(channels[THROTTLE]); _aux1->pulsewidth_us( channels[AUX1]); _aux2->pulsewidth_us( channels[AUX2]); } Thread::wait(20); } }