//=============================================================================================================================================== /* * This sketch provides basic control over the testbed for the IMU of your project. Except where indicated, feel free to change it around * however you need to. * */ #include #include #include "Wire.h" #include "LiquidCrystal.h" // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // *** DO NOT CHANGE ANYTHING HERE *** #define MOTOR_DRIVE_A 3 // the PWM output pin for the A channel of the H-bridge #define MOTOR_DRIVE_B 5 // the PWM output pin for the B channel of the H-bridge #define ENCODER_PIN 13 // the quadrature encoder input pin from the motor #define PAN_PIN 10 // the pan servo PWM output pin #define TILT_PIN 11 // the tilt servo PWM output pin #define JOYSTICK_VERTICAL_PIN 1 // the joystick vertical input pin #define PUSHBUTTON_PIN 4 // the pushbutton input pin #define MOTOR_PWM_SCALER 32 // the PWM scaler for the H-bridge #define PAN_HOME 1500 // the home, minimum, and maximum PWM frequencies for the pan servo #define PAN_MIN 600 #define PAN_MAX 2400 #define PAN_TEST 1200 #define TILT_HOME 1500 // the home, minimum, and maximum PWM frequencies for the tilt servo #define TILT_MIN 1200 #define TILT_MAX 1800 Servo panServo; // the pan and tilt servos Servo tiltServo; LiquidCrystal lcd(0); // the LCD display //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Sets up the Arduino. This function is called automatically upon startup, followed by loop(). It is ok to add your own setup code, but be * careful about changing anything. */ void setup() { lcd.begin(16, 4); lcd.print("Starting..."); // initialize the servos panServo.attach(PAN_PIN, PAN_MIN, PAN_MAX); tiltServo.attach(TILT_PIN, TILT_MIN, TILT_MAX); // initalize the pins pinMode(ENCODER_PIN, INPUT); pinMode(PUSHBUTTON_PIN, INPUT); pinMode(MOTOR_DRIVE_A, OUTPUT); pinMode(MOTOR_DRIVE_B, OUTPUT); stopMotor(); setPWMFrequency(MOTOR_DRIVE_A, MOTOR_PWM_SCALER); setPWMFrequency(MOTOR_DRIVE_B, MOTOR_PWM_SCALER); // run a quick selftest to show that the program has started panServo.writeMicroseconds(PAN_TEST); delay(1000); setGimbalHome(); delay(1000); setInitialPosition(); lcd.clear(); lcd.print("Running tests..."); delay(1000); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Calls your tests. This function is called automatically by the Arduino after setup(). It will continue to be called unless prevented. */ void loop() { doYourGimbalTest(); doYourMotorTest(); // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // leave the Arduino in an infinite loop in the final state it achieved; otherwise, it will keep rerunning this function setGimbalHome(); lcd.clear(); lcd.print("Tests complete"); while (true); // *** DO NOT REMOVE THIS LINE *** } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Executes a gimbal test. */ void doYourGimbalTest() { setGimbalYaw(-45); setGimbalPitch(+30); // may not got to +30 because of limits } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Executes a motor test. */ void doYourMotorTest() { // the bow seems to be about 1200 pulses long moveMotorForward(100, 600); moveMotorBackward(100, 1200); moveMotorForward(100, 600); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Moves the gimbal to its home position in pitch and yaw. */ void setGimbalHome() { panServo.writeMicroseconds(PAN_HOME); tiltServo.writeMicroseconds(TILT_HOME); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Sets the gimbal yaw. After calling this, you may need to call delay(milliseconds) to give the servo time to reach its target. * * angle - the angle in degrees [-90,+90] */ void setGimbalYaw(int angle) { assert ((angle >= -90) && (angle <= +90)); int frequency = map(angle, -90, +90, PAN_MIN, PAN_MAX); frequency = min(frequency, PAN_MAX); frequency = max(frequency, PAN_MIN); panServo.writeMicroseconds(frequency); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Sets the gimbal pitch. After calling this, you may need to call delay(milliseconds) to give the servo time to reach its target. * * angle - the angle in degrees [-90,+90]; the actual range is more limited to avoid hitting the bow against the table */ void setGimbalPitch(int angle) { assert ((angle >= -90) && (angle <= +90)); int frequency = map(angle, -90, +90, TILT_MIN, TILT_MAX); frequency = min(frequency, TILT_MAX); frequency = max(frequency, TILT_MIN); tiltServo.writeMicroseconds(frequency); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Moves the motor forward a distance at a speed. * * speed - the speed [0,255] * distance - the positive distance, which corresponds to the number of encoder pulses on the motor */ void moveMotorForward(unsigned int speed, unsigned long distance) { assert ((speed >=0) && (speed <= 255)); analogWrite(MOTOR_DRIVE_B, 0); analogWrite(MOTOR_DRIVE_A, 255); delay(25); analogWrite(MOTOR_DRIVE_A, speed); waitDistance(distance); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Moves the motor backward a distance at a speed. * * speed - the speed [0,255] * distance - the positive distance, which corresponds to the number of encoder pulses on the motor */ void moveMotorBackward(unsigned int speed, unsigned long distance) { assert ((speed >=0) && (speed <= 255)); analogWrite(MOTOR_DRIVE_A, 0); analogWrite(MOTOR_DRIVE_B, 255); delay(25); analogWrite(MOTOR_DRIVE_B, speed); waitDistance(distance); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Stops the motor. */ void stopMotor() { analogWrite(MOTOR_DRIVE_A, 0); analogWrite(MOTOR_DRIVE_B, 0); } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Waits for the motor to rotate a certain distance. * * distance - the positive distance, which corresponds to the number of encoder pulses on the motor */ void waitDistance(unsigned long distance) { assert (distance > 0); unsigned long encoderPulses = 0; unsigned int encoderPulse = 0; unsigned int encoderPulsePrev = -1; while (true) { encoderPulse = digitalRead(ENCODER_PIN); if (encoderPulse != encoderPulsePrev) { encoderPulses++; if (encoderPulses >= distance) { stopMotor(); return; } } encoderPulsePrev = encoderPulse; } } //----------------------------------------------------------------------------------------------------------------------------------------------- /* * Reads the joystick to center the bow. Press the button when ready to continue. */ void setInitialPosition() { lcd.clear(); lcd.print("Use joystick to"); lcd.setCursor(0,1); lcd.print("center bow, then"); lcd.setCursor(0,2); lcd.print("press button"); while (true) { int value = analogRead(JOYSTICK_VERTICAL_PIN); if (value > 700) { moveMotorForward(50, 1); } else if (value < 300) { moveMotorBackward(50, 1); } int buttonState = digitalRead(PUSHBUTTON_PIN); if (buttonState == HIGH) { lcd.clear(); break; } delay(25); } } //----------------------------------------------------------------------------------------------------------------------------------------------- /** * *** DO NOT TOUCH THIS FUNCTION *** * * Sets the frequency of the pulse-width modulation for the motor. * * pin - the PWM pin * divisor - the frequency divisor, which depends on the pin */ void setPWMFrequency(int pin, int divisor) { byte mode; if ((pin == 5) || (pin == 6) || (pin == 9) || (pin == 10)) { switch (divisor) { case 1: mode = 0x01; break; case 8: mode = 0x02; break; case 64: mode = 0x03; break; case 256: mode = 0x04; break; case 1024: mode = 0x05; break; default: return; } if ((pin == 5) || (pin == 6)) { TCCR0B = (TCCR0B & 0b11111000 | mode); } else { TCCR1B = (TCCR1B & 0b11111000 | mode); } } else if ((pin == 3) || (pin == 11)) { switch (divisor) { case 1: mode = 0x01; break; case 8: mode = 0x02; break; case 32: mode = 0x03; break; case 64: mode = 0x04; break; case 128: mode = 0x05; break; case 256: mode = 0x06; break; case 1024: mode = 0x07; break; default: return; } TCCR2B = (TCCR2B & 0b11111000 | mode); } }