////////////////////////////////////////////////////////////////////////////////////////////////////////// // PicoMotorPPM for Pigeon Control (PMPPMPC) // // Software for group assignment, FabAcademy, Waag team 2024 // #include // use Adafruit's PCA9685 driver, but this time not for RC servos but plain PWM on the WaveShare H-bridge #include Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); #define DEADZONE 5 // otherwise motors keep humming... // Initialize a PPMReader on digital pin 3 with 6 expected channels. byte PPMpin = 3; // the pin for PPM radio input byte channelAmount = 6;// number of channels on receiver // and for the audio module #include #define DF1201SSerial Serial2 DFRobot_DF1201S DF1201S; // and the code starts here: void setup() { Serial.begin(115200); // Our Debug Interface pinMode(25, OUTPUT); // Pico on board LED attachInterrupt(PPMpin, ISR, FALLING);// PPM input pin // pinMode(26,OUTPUT); // the H-bridge for pigeon motor // pinMode(27,OUTPUT); // the H-bridge for pigeon motor //digitalWrite(26,OUTPUT); // audio stuff Serial2.setTX(4); // Serial for the audio player Serial2.setRX(5); // Serial for the audio player DF1201SSerial.begin(115200); while (!DF1201S.begin(DF1201SSerial)) { Serial.println("Init failed, please check the wire connection!"); delay(1000); } DF1201S.switchFunction(DF1201S.MUSIC); DF1201S.setPrompt(false); DF1201S.setPlayMode(DF1201S.SINGLE); DF1201S.setVol(/*VOL = */ 20); /// next up Wire.setSDA(20); // redirect for WaveShare board pinout Wire.setSCL(21); // redirect for WaveShare board pinout pwm.begin(); // start the PCA9685 pwm.setOscillatorFrequency(27000000); // base frequency pwm.setPWMFreq(16000); // check which modes still work. Faster PWM means less noise means more switching loss Wire.setClock(400000); // can be fast } bool playing = 0; void loop() { int values [] = {0,0,0,0,0,0}; for (byte channel = 1; channel <= channelAmount; ++channel) { values [channel-1] = (latestValidChannelValue(channel, 1550)-1550); // 1550 is the current center value Serial.print(values[channel-1]); if (channel < channelAmount) Serial.print('\t'); } Serial.println(); if(values[2] > -300){ // failsafe on throttle channel float frontrot = 0.4; // rotation mode (around pigeon center float backrot = 1.0; // rotation mode (around pigeon center if (values[4]>100) {frontrot = -0.4; backrot=-1.0;}else {frontrot = 1.0;backrot = 1.0;} // else translation setMotor(0, map((values[1])-(frontrot*(values[0]))-(values[3]), -350, 350, -255, 255)); setMotor(1, map((values[1])+(frontrot*(values[0]))+(values[3]), -350, 350, -255, 255)); setMotor(2, map((values[1])-(backrot*(values[0]))+(values[3]), -350, 350, -255, 255)); setMotor(3, map((values[1])+(backrot*(values[0]))-(values[3]), -350, 350, -255, 255)); // analogWrite(26,map(values[2],-430,430,0,255)); if(!playing && values[5]>100){ playing = 1; //DF1201S.playSpecFile("coo02.mp3"); //DF1201S.playSpecFile("coo02.mp3"); DF1201S.playFileNum(1); Serial.println("play 1"); } if(playing && values[5]<100 && values[5]>-100){ playing = 0; DF1201S.pause(); Serial.println("pause"); } if(!playing && values[5]<-100){ playing = 1; //DF1201S.playSpecFile("coo01.mp3"); //DF1201S.playSpecFile("coo02.mp3"); DF1201S.playFileNum(2); Serial.println("play 2"); } } else { setMotor(0,0); setMotor(1,0); setMotor(2,0); setMotor(3,0); } delay(20); } // for the Waveshare PCA9685 the following channel assignment is used // MA MB MC MD int motorpins[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; void setMotor(int motor, int value) { if(value>255) value = 255; if(value<-255) value = -255; if (value > DEADZONE) { pwm.setPWM(motorpins[motor*3], 0, map(value, 0, 255, 0, 4095)); pwm.setPWM(motorpins[motor*3+1], 0, 4095); pwm.setPWM(motorpins[motor*3+2], 0, 0); } else if (value < -DEADZONE) { pwm.setPWM(motorpins[motor*3], 0, map(-value, 0, 255, 0, 4095)); pwm.setPWM(motorpins[motor*3+1], 0, 0); pwm.setPWM(motorpins[motor*3+2], 0, 4095); } else { pwm.setPWM(motorpins[motor*3], 0, 0); pwm.setPWM(motorpins[motor*3+1], 0, 0); pwm.setPWM(motorpins[motor*3+2], 0, 0); } } //////////////////////////////////////////////////////////////////////////////////////////////////////// // // PPMreader code // volatile unsigned long microsAtLastPulse = 0; // A counter variable for determining which channel is being read next volatile byte pulseCounter = 0; // Arrays for keeping track of channel values volatile unsigned rawValues [6]; // The range of a channel's possible values (microseconds) unsigned minChannelValue = 1000; unsigned maxChannelValue = 2000; // The maximum error (in either direction) in channel value with which the channel value is still considered valid unsigned channelValueMaxError = 10; // The minimum blanking time (microseconds) after which the frame current is considered to be finished // Should be bigger than maxChannelValue + channelValueMaxError unsigned blankTime = 2100; // The timeout (microseconds) after which the channels which were not updated are considered invalid unsigned long failsafeTimeout = 200000L; // 200 mS int rawChannelValue(byte channel) { // Check for channel's validity and return the latest raw channel value or 0 unsigned value = 0; if (channel >= 1 && channel <= channelAmount) { noInterrupts(); value = rawValues[channel - 1]; interrupts(); } return value; } int latestValidChannelValue(byte channel, unsigned defaultValue) { // Check for channel's validity and return the latest valid channel value or defaultValue. unsigned value = defaultValue; unsigned long timeout; noInterrupts(); timeout = micros() - microsAtLastPulse; interrupts(); if ((timeout < failsafeTimeout) && (channel >= 1) && (channel <= channelAmount)) { noInterrupts(); value = rawValues[channel - 1]; interrupts(); if (value >= minChannelValue - channelValueMaxError && value <= maxChannelValue + channelValueMaxError) { value = constrain(value, minChannelValue, maxChannelValue); } else value = defaultValue; } return value; } void ISR() { if (digitalRead(25) == 1) digitalWrite(25, LOW); else digitalWrite(25, HIGH); unsigned long previousMicros = microsAtLastPulse; microsAtLastPulse = micros(); unsigned long time = microsAtLastPulse - previousMicros; if (time > blankTime) { // Blank detected: restart from channel 1 pulseCounter = 0; } else { // Store times between pulses as channel values if (pulseCounter < channelAmount) { rawValues[pulseCounter] = time; ++pulseCounter; } } }