From 1259cb041f80c86941086e5dfa6c9cf2d007da5c Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Tue, 28 Dec 2010 20:57:03 +0000 Subject: [PATCH] Improved AP_RcChannel examples. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1331 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- .../AdvUsage.pde} | 0 .../examples/Manual/ServoSweep.pde | 87 +++++++++++++++ .../examples/ServoSweep/ServoSweep.pde | 100 ++++++++++++++++++ 3 files changed, 187 insertions(+) rename libraries/AP_RcChannel/examples/{AP_RcChannel/AP_RcChannel.pde => AdvUsage/AdvUsage.pde} (100%) create mode 100644 libraries/AP_RcChannel/examples/Manual/ServoSweep.pde create mode 100644 libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde diff --git a/libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde b/libraries/AP_RcChannel/examples/AdvUsage/AdvUsage.pde similarity index 100% rename from libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde rename to libraries/AP_RcChannel/examples/AdvUsage/AdvUsage.pde diff --git a/libraries/AP_RcChannel/examples/Manual/ServoSweep.pde b/libraries/AP_RcChannel/examples/Manual/ServoSweep.pde new file mode 100644 index 0000000000..ab4e6892a4 --- /dev/null +++ b/libraries/AP_RcChannel/examples/Manual/ServoSweep.pde @@ -0,0 +1,87 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + +*/ + +#include +#include +#include // ArduPilot Mega RC Library +#include +#include + +// test settings +uint8_t nChannels = 8; +bool loadFromEEProm = false; + +// channel configuration +Vector< AP_EEPromVar * > scale; +Vector< AP_EEPromVar * > pwmMin; +Vector< AP_EEPromVar * > pwmNeutral; +Vector< AP_EEPromVar * > pwmMax; +Vector< AP_EEPromVar * > pwmDeadZone; +Vector< AP_Var * > filter; +Vector< AP_Var * > reverse; +Vector< AP_RcChannel * > rc ; + +// test position +float testPosition = 0; +uint16_t testPwm = 1500; +int8_t testSign = 1; + +// serial +FastSerialPort0(Serial); + +void setup() +{ + Serial.begin(115200); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + + // add channels + for (int i=0;i(1.0,strcat("SCALE",num))); + pwmMin.push_back(new AP_EEPromVar(1200,strcat("PWM_MIN",num))); + pwmNeutral.push_back(new AP_EEPromVar(1500,strcat("PWM_NEUTRAL",num))); + pwmMax.push_back(new AP_EEPromVar(1800,strcat("PWM_MAX",num))); + pwmMax.push_back(new AP_EEPromVar(10,strcat("PWM_DEADZONE",num))); + + // load + if (loadFromEEProm) + { + scale[i]->load(); + pwmMin[i]->load(); + pwmNeutral[i]->load(); + pwmMax[i]->load(); + pwmDeadZone[i]->load(); + filter[i]->load(); + reverse[i]->load(); + } + + // add rc channel + rc.push_back(new AP_RcChannel(APM_RC,i,scale[i]->get(), + pwmMin[i]->get(),pwmNeutral[i]->get(),pwmMax[i]->get(), + pwmDeadZone[i]->get(),filter[i]->get(),reverse[i]->get())); + + // find neutral position + rc[i]->readRadio(); + pwmNeutral[i]->set(rc[i]->getPwm()); + } +} + +void loop() +{ + // set channel positions + for (int i=0;i<7;i++) + { + rc[i]->getRadio(); + Serial.printf("\npwm:\t%d\t\tposition:\t%f\n", rc[i]->getPwm(),rc[i]->getPosition()); + } + delay(100); +} diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde new file mode 100644 index 0000000000..5a1e6c2551 --- /dev/null +++ b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde @@ -0,0 +1,100 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + +*/ + +#include +#include +#include // ArduPilot Mega RC Library +#include +#include + +// test settings +uint8_t nChannels = 8; +bool loadFromEEProm = false; + +// channel configuration +Vector< AP_EEPromVar * > scale; +Vector< AP_EEPromVar * > pwmMin; +Vector< AP_EEPromVar * > pwmNeutral; +Vector< AP_EEPromVar * > pwmMax; +Vector< AP_EEPromVar * > pwmDeadZone; +Vector< AP_Var * > filter; +Vector< AP_Var * > reverse; +Vector< AP_RcChannel * > rc ; + +// test position +float testPosition = 0; +uint16_t testPwm = 1500; +int8_t testSign = 1; + +// serial +FastSerialPort0(Serial); + +void setup() +{ + Serial.begin(115200); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + + // add channels + for (int i=0;i(1.0,strcat("SCALE",num))); + pwmMin.push_back(new AP_EEPromVar(1200,strcat("PWM_MIN",num))); + pwmNeutral.push_back(new AP_EEPromVar(1500,strcat("PWM_NEUTRAL",num))); + pwmMax.push_back(new AP_EEPromVar(1800,strcat("PWM_MAX",num))); + pwmMax.push_back(new AP_EEPromVar(10,strcat("PWM_DEADZONE",num))); + + // load + if (loadFromEEProm) + { + scale[i]->load(); + pwmMin[i]->load(); + pwmNeutral[i]->load(); + pwmMax[i]->load(); + pwmDeadZone[i]->load(); + filter[i]->load(); + reverse[i]->load(); + } + + // add rc channel + rc.push_back(new AP_RcChannel(APM_RC,i,scale[i]->get(), + pwmMin[i]->get(),pwmNeutral[i]->get(),pwmMax[i]->get(), + pwmDeadZone[i]->get(),filter[i]->get(),reverse[i]->get())); + + // find neutral position + rc[i]->readRadio(); + pwmNeutral[i]->set(rc[i]->getPwm()); + } +} + +void loop() +{ + // set channel positions + for (int i=0;i<7;i++) + { + rc[i]->setPosition(testPosition); + Serial.printf("\npwm:\t%d\t\tposition:\t%f\n", rc[i]->getPwm(),rc[i]->getPosition()); + } + + // update test value + testPosition += testSign*.05; + if (testPosition > 1) + { + testPosition = -1; + testSign = -1; + } + else if (testPosition < 1) + { + testPosition = 1; + testSign = 1; + } + delay(500); +}