/* Control Panel for Rover Arduino NANO Potentiometer A0, A1 for THROTTLE and ROLL A3, A4, A5 for digital input for ARM/DISARM, MOTOR EMERGENCY STOP, LEARN WAYPOINT D2, D3, D4, D5, D6, D7 for servo output representing channels 1 (roll), 3 (throttle), 5 (emergency motor stop), 7 (learn waypoint), 8 (flight mode) D9, D10, D11 for mode "B" selection, D12 for HOLD/mode "B" selection */ #include // constants const int adcValMin = 0; //normal minimum adc reading const int adcValMax = 1023; //normal maximum adc reading const int adcValThrDZMin = 475; // dead zone for throttle - min (measured from experiment) const int adcValThrDZMax = 535; // dead zone for throttle - max (measured from experiment) const int adcValRollDZMin = 480; // dead zone for roll - min (measured from experiment) const int adcValRollDZMax = 535; // dead zone for roll - max (measured from experiment) const int servoValMin = 1000; //millisec const int servoValMid = 1500; //millisec const int servoValMax = 2000; //millisec const int servoValMode1 = 1165; //millisec (Mission Planner Mode 1 = HOLD) - control panel = HOLD switch const int servoValMode2 = 1295; //millisec (Mission Planner Mode 2 = MANUAL) - control panel = Mode B switch position 1 const int servoValMode3 = 1425; //millisec (Mission Planner Mode 3 = HOLD) - control panel = not used const int servoValMode4 = 1555; //millisec (Mission Planner Mode 4 = AUTO) - control panel = Mode B switch position 2 const int servoValMode5 = 1685; //millisec (Mission Planner Mode 5 = HOLD) - control panel = not used const int servoValMode6 = 1815; //millisec (Mission Planner Mode 6 = STEERING) - control panel = Mode B switch position 3 //INPUT PINS int analogPinThrottle = A0; //analog pin for reading THROTTLE potentiometer int analogPinRoll = A1; //analog pin for reading ROLL potentiometer int switchArm = A3; //ARM / DISARM switch (analog used as digital) int switchEmStop = A4; //Emergency Motor Stop switch (analog used as digital) int switchLearn = A5; //Learn Waypoint switch (analog used as digital) int switchPos1 = 9; //Mode B switch position 1 - Label = MANUAL int switchPos2 = 10; //Mode B switch position 2 - Label = AUTO int switchPos3 = 11; //Mode B switch position 3 - Label = STEER int switchHold = 12; //Hold / Mode B switch //OUTPUT PINS int OutCh1Roll = 2; //pin for output ROLL pulses - Ch1 int OutCh3Throttle = 3; //pin for output THROTTLE pulses - Ch3 int OutCh5MotorCut = 4; //pin for output Motor Emergency Cut pulses - Ch5 int OutCh6Arm = 5; //pin for output ARM / DISARM pulses - Ch6 int OutCh7Learn = 6; //pin for output LEARN WP pulses - Ch7 int OutChMode = 7; //pin for output FLIGHT MODE pulses - Ch8 // Variables int adcRoll = 512; //mid-point to start int adcThrottle = 512; //mid-point to start int switchState; //state of current button (ie HIGH or LOW) int adcRollOld = 512; //holds previous value int adcThrottleOld = 512; //holds previous value int switchEmStopOld = 1000; //holds previous state int switchArmOld = 1000; //holds previous state int switchHoldOld = 1165; //holds previous state int switchPos1Old = 1165; //holds previous state int switchPos2Old = 1165; //holds previous state int switchPos3Old = 1165; //holds previous state int servoValCh1; //output for servo channel 1 in mSec int servoValCh3; //output for servo channel 3 in mSec int servoValCh5; //output for servo channel 5 in mSec int servoValCh6; //output for servo channel 6 in mSec int servoValCh7; //output for servo channel 7 in mSec int servoValCh8; //output for servo channel 8 in mSec //Declarations Servo servoCh1; // Roll Servo servoCh3; // Throttle Servo servoCh5; // Emergency motor cut Servo servoCh6; // Arm / Disarm Servo servoCh7; // Learn waypoint Servo servoCh8; // Flight mode //==================================================================================================== void setup() { Serial.begin(9600); // initialize serial comms 9600 baud //INPUT PINS pinMode(switchHold, INPUT_PULLUP); // Hold / Mode B switch pinMode(switchPos1, INPUT_PULLUP); // SwitchPosition 1 pinMode(switchPos2, INPUT_PULLUP); // SwitchPosition 2 pinMode(switchPos3, INPUT_PULLUP); // SwitchPosition 3 pinMode(switchArm, INPUT_PULLUP); // Learn Waypoint button pinMode(switchEmStop, INPUT_PULLUP); // Emergency motor stop Switch pinMode(switchLearn, INPUT_PULLUP); // Arm / Disarm Switch //OUTPUT PINS servoCh1.attach(OutCh1Roll); // Roll output on pin 0 servoCh3.attach(OutCh3Throttle); // Throttle output on pin 1 servoCh5.attach(OutCh5MotorCut); // Emergency motor cut output on pin 2 servoCh6.attach(OutCh6Arm); // Arm / Disarm output on pin 3 servoCh7.attach(OutCh7Learn); // Learn waypoint output on pin 4 servoCh8.attach(OutChMode); // Flight mode output on pin 5 //SET SERVOS TO STARTUP VALUES servoCh1.writeMicroseconds(servoValMid); // Roll off servoCh3.writeMicroseconds(servoValMid); // Throttle off servoCh5.writeMicroseconds(servoValMin); // Emergency motor cut - run servoCh6.writeMicroseconds(servoValMin); // Arm / Disarm - disarm servoCh7.writeMicroseconds(servoValMin); // Learn waypoint - off servoCh8.writeMicroseconds(servoValMode1); // Flight mode - mode 1, HOLD // ON BOARD LED OFF pinMode(LED_BUILTIN, OUTPUT); // digitalWrite(LED_BUILTIN, LOW); } //==================================================================================================== void loop() { //---Channel 1 roll----------------------------------------------------------------------------------- adcRoll = analogRead(analogPinRoll); // read the input on analog pin A1, Roll if (abs(adcRoll-adcRollOld) > 1) // run code only if value has changed { adcRollOld = adcRoll; // save new value of reading if (adcRoll >= adcValRollDZMin and adcRoll <= adcValRollDZMax) // set output at 1500 if stick is in dead zone { servoValCh1 = servoValMid; } else { servoValCh1 = map(adcRoll, adcValMin, adcValMax, servoValMin, servoValMax); } servoCh1.writeMicroseconds(servoValCh1); // output to channel 1 servo } //---Channel 3 throttle----------------------------------------------------------------------------------- adcThrottle = analogRead(analogPinThrottle); // read the input on analog pin A0, Throttle if (abs(adcThrottle-adcThrottleOld) > 1) // run code only if value has changed { adcThrottleOld = adcThrottle; // save new value of reading if (adcThrottle >= adcValThrDZMin && adcThrottle <= adcValThrDZMax) // set output at 1500 if stick is in dead zone { servoValCh3 = servoValMid; } else { servoValCh3 = map(adcThrottle, adcValMin, adcValMax, servoValMin, servoValMax); } servoCh3.writeMicroseconds(servoValCh3); // output to channel 3 servo } //---Channel 5 emergency stop----------------------------------------------------------------------------------- switchState = digitalRead(switchEmStop); if (switchState != switchEmStopOld) // run code only if switch state has changed { switchEmStopOld = switchState; // remember current state if (switchState == LOW) // if the EMERGENCY MOTOR STOP switch is closed (= low) then send stop value to channel 5 { servoValCh5 = servoValMax; // stops motor on high RC signal } else { servoValCh5 = servoValMin; } servoCh5.writeMicroseconds(servoValCh5); } //---Channel 6 arm/disarm----------------------------------------------------------------------------------- switchState = digitalRead(switchArm); if (switchState != switchArmOld) // run code only if switch state has changed { switchArmOld = switchState; // remember current state if (switchState == LOW) // if the ARM/DISARM switch is closed (= low) then send DISARM value to channel 6 { servoValCh6 = servoValMin; // disarms autopilot on low RC signal } else { servoValCh6 = servoValMax; // arms autopilot on high RC signal } servoCh6.writeMicroseconds(servoValCh6); } //---Channel 7 learn waypoint-----------------------------------------------// NOTE: this is a momentary normally open button if (servoValCh8 != servoValMode4) // don't learn waypoint in AUTO mode { if (digitalRead(switchLearn) == LOW) // if the LEARN WAYPOINT button is pressed (= low) then send learn value to channel 7 { while (digitalRead(switchLearn) == LOW) // debounce button, wait for button to be released { delay(10); } servoCh7.writeMicroseconds(servoValMax); digitalWrite(LED_BUILTIN, HIGH); // flash LED on pin 13 to acknowledge point saved delay(500); // hold channel output high to enable time to register on Pixhawk digitalWrite(LED_BUILTIN, LOW); servoCh7.writeMicroseconds(servoValMin); } } //---Channel 8 flight mode----------------------------------------------------------------------------------- if ( digitalRead(switchHold) != switchHoldOld // run code only if any mode switches have changed or digitalRead(switchPos1) != switchPos1Old or digitalRead(switchPos2) != switchPos2Old or digitalRead(switchPos3) != switchPos3Old ) { switchHoldOld = digitalRead(switchHold); // remember current state of HOLD switch switchPos1Old = digitalRead(switchPos1); // remember current state of HOLD switch switchPos2Old = digitalRead(switchPos2); // remember current state of HOLD switch switchPos3Old = digitalRead(switchPos3); // remember current state of HOLD switch if (digitalRead(switchHold) == HIGH) // if the HOLD switch is not closed (= high) send HOLD value to channel 8 { servoValCh8 = servoValMode1; // Mode 1 = HOLD } else { // otherwise ch8 value determined by Mode B 3-way switch if (digitalRead(switchPos1) == LOW) // if the 3-way switch is in position 1 (bottom) { servoValCh8 = servoValMode2; // Mode 2 = MANUAL } if (digitalRead(switchPos2) == LOW) // if the 3-way switch is in position 2 (middle) { servoValCh8 = servoValMode4; // Mode 4 = AUTO } if (digitalRead(switchPos3) == LOW) // if the 3-way switch is in position 3 (top) { servoValCh8 = servoValMode6; // Mode 6 = STEERING } } servoCh8.writeMicroseconds(servoValCh8); } //--------------------------------------------------------------------------------------------------------------- } // end of main loop //================================================================================================================