/** * Aansturing micromotor met RC kanaal * Author: Shawn Hymel (SparkFun Electronics) * Date: Aug 17, 2017 * Aangepast Toin van Daal * Datum 24-06-2019 * Definitief Gerrit Volgers * Datum: 5-8-2019 * Motor A is voor heffen en blokkeert in opgevouwen stand de andere * Motor B Zwenken * Motor C Telescoop * * Verbind RC (PWM) ontvanger met de Arduino. * 1 kanaal voor vooruit, achteruit en snelheid */ #define kanaalnummer_5 // const int CHANNEL_5; // Lees kanaal nummer 6: Functie telescoperen // Controller pins const int CH_5_PIN = 4; // Motor driver pins const int AIN1_PIN = 2; const int AIN2_PIN = 1; const int APWM_PIN = 3; #define kanaalnummer_4 const int CHANNEL_4;// Lees kanaal nummer 4. Functie: heffen // Controller pins const int CH_4_PIN = 8; // Motor driver pins const int BIN1_PIN = 6; const int BIN2_PIN = 7; const int BPWM_PIN = 5; #define kanaalnummer_3 const int CHANNEL_3;// Lees kanaal nummer 3. Functie:zwenken // Controller pins const int CH_3_PIN = 12; // Motor driver pins const int A2IN1_PIN = 9; const int A2IN2_PIN = 10; const int A2PWM_PIN = 11; // Parameters const int deadzone =30; // Anything between -30 and 30 is stop. Experimenteel bepalen: zo klein als mogelijk/zinvol const int eindstop_pin = 14; // diconst int STBY_PIN = 15; // dit is pin A0 gebruikt als digitaal input 14 voor test const int STBY_PIN = 15; // dit is pin A0 gebruikt als digitaal input 15. (Optioneel) void setup() { // Configure pins motor A pinMode(CH_5_PIN, INPUT); pinMode(AIN1_PIN, OUTPUT); pinMode(AIN2_PIN, OUTPUT); pinMode(APWM_PIN, OUTPUT); // Configure pins motor B pinMode(CH_3_PIN, INPUT); pinMode(BIN1_PIN, OUTPUT); pinMode(BIN2_PIN, OUTPUT); pinMode(BPWM_PIN, OUTPUT); // Configure pins motor A2 pinMode(CH_4_PIN, INPUT); pinMode(A2IN1_PIN, OUTPUT); pinMode(A2IN2_PIN, OUTPUT); pinMode(A2PWM_PIN, OUTPUT); // Configure other pins pinMode(eindstop_pin, INPUT); pinMode(STBY_PIN, INPUT); // Enable motor drivers digitalWrite(STBY_PIN, HIGH); //Dit is nodig om de motordriver 1 te aktiveren (optioneel) } void loop() { // Read pulse width from receiver int ch_5 = pulseIn(CH_5_PIN, HIGH, 50000); //Min 50ms tussen twee pulsen van kanaal 2 int ch_3 = pulseIn(CH_3_PIN, HIGH, 50000); //Min 50ms tussen twee pulsen van kanaal 3 int ch_4 = pulseIn(CH_4_PIN, HIGH, 50000); //Min 50ms tussen twee pulsen van kanaal 4 // Convert to PWM value (-255 to 255) ch_5 = pulseToPWM(ch_5); ch_3 = pulseToPWM(ch_3); ch_4 = pulseToPWM(ch_4); if ( digitalRead(eindstop_pin) == LOW ) { ch_4 = 0; ch_5 = 0 ; // motor b en A2 geblokkeerd if ( ch_3 > 0 ) { ch_3 = 0 ;} // motor A kan maar één kant op als de eindstop laag is: kraan heffen } // Drive motor drive5(ch_5); drive3(ch_3); drive4(ch_4); delay(5); } // Positive for forward, negative for reverse void drive5(int speed_a) { // Limit speed between -255 and 255 speed_a = constrain(speed_a, -255, 255); // Set direction for motor A if ( speed_a == 0 ) { digitalWrite(AIN1_PIN, LOW); digitalWrite(AIN2_PIN, LOW); } else if ( speed_a > 0 ) { digitalWrite(AIN1_PIN, HIGH); digitalWrite(AIN2_PIN, LOW); } else { digitalWrite(AIN1_PIN, LOW); digitalWrite(AIN2_PIN, HIGH); } // Set speed analogWrite(APWM_PIN, abs(speed_a)); } void drive3(int speed_b) { // Limit speed between -255 and 255 speed_b = constrain(speed_b, -255, 255); // Set direction for motor B if ( speed_b == 0 ) { digitalWrite(BIN1_PIN, LOW); digitalWrite(BIN2_PIN, LOW); } else if ( speed_b > 0 ) { digitalWrite(BIN1_PIN, HIGH); digitalWrite(BIN2_PIN, LOW); } else { digitalWrite(BIN1_PIN, LOW); digitalWrite(BIN2_PIN, HIGH); } // Set speed analogWrite(BPWM_PIN, abs(speed_b)); } void drive4(int speed_a2) { // Limit speed between -255 and 255 speed_a2 = constrain(speed_a2, -255, 255); // Set direction for motor A2 if ( speed_a2 == 0 ) { digitalWrite(A2IN1_PIN, LOW); digitalWrite(A2IN2_PIN, LOW); } else if ( speed_a2 > 0 ) { digitalWrite(A2IN1_PIN, HIGH); digitalWrite(A2IN2_PIN, LOW); } else { digitalWrite(A2IN1_PIN, LOW); digitalWrite(A2IN2_PIN, HIGH); } // Set speed analogWrite(A2PWM_PIN, abs(speed_a2)); } // Convert RC pulse value to motor PWM value int pulseToPWM(int pulse) { // If we're receiving numbers, convert them to motor PWM if ( pulse > 1000 ) { pulse = map(pulse, 1000, 2000, -500, 500); pulse = constrain(pulse, -255, 255); } else { pulse = 0; } // Anything in deadzone should stop the motor if ( abs(pulse) <= deadzone ) { pulse = 0; } return pulse; }