Matlab Code for Ball Launch Calculations
For the first project checkpoint, we produced a set of calculations governing how the nerf balls would move as our robot launched them. Below is the Matlab code that produced those calculations.
%% SHOOTING THE BALL a = 9.81 %m/s^2 t = linspace(0,.5,1000) %s v = 0 %s %assuming no air resistance. %how far will it drop? d = t.*v + 1/2*(a*t.^2) plot(t, d) xlabel('Time (s)') ylabel('Drop in height (m)') v = 1.2/.15 %vmin is cap at 10cm drop across 4m %take motor speed and calculate contact %distance and time. Assume radius and w. w = 3000 % rpmdspin = .06 %m m = .002 %kg dspin = dspin*pi/2 w = w*2*pi/60 %rad/sec trad = pi/w %Assume 9V battery. %Calculate power necessary to accelerate %assume all power transfer to ball ain = v/trad Pin = ain*dspin*m %Watts %with 40% effective motor %assuming all power tranfer to ball Pi = Pin/.40 %assuming 12v battery i = Pi/12 %% MOVEMENT %robot estimated max is 10 lbs(15 allowed max) m2 = 4.5 %kg %how fast do we want it to move? %10 s to cross 8 feet for now. v2 = 2.4/20 %m/s w2 = v2/(dspin*2) %rps %ratio from motor ratio = 30 wshaft = w2*60 %rpm figure W = [200,160] %RPM T = [0,.083] %Nm x = linspace(0,.415, 415) Fit = polyfit(T, W, 1) %Best fit line. 1 parameter means linear plot(x, polyval(Fit, x)) xlabel('Torque (Nm)') ylabel('W (rpm)') Tout = .337 %Nm wrad = wshaft/60*2*pi Pout = wrad*Tout Pinmotor = Pout/.40 i2 = Pinmotor/12
Main Arduino Code for Robot
/*---------Includes----------*/ #include <NewPing.h> #include <Timers.h> #include <Servo.h> /*----------Pin Definitions---------------*/ //Motor Pins //North black (NB), north red (NR), south black (SB), south red (SR) //Note: These are all PWM pins on the MEGA #define NR 6 #define NB 5 #define SR 7 #define SB 8 #define WB 4 #define WR 3 #define ER 9 #define EB 10 //Sensor Pins //O = trigger (WNO = West North trigger, etc.) //I = echo #define WNO 2 #define WNI 30 #define WSO 2 #define WSI 24 #define SEO 2 #define SEI 11 #define SWO 2 #define SWI A1 //Flywheel Control Driver Pins #define FLYWHEEL_PIN_HIGH 12 #define FLYWHEEL_PIN_LOW 13 //Servo Control Pin #define SERVO_PIN A3 /*----------Constant Definitions---------------*/ #define BAUD_RATE 115200 #define SERVO_GATE_ANGLE_CHANGE 30 #define SECOND 1000 //SPEEDO is Speed Orient, the speed we use when the robot is aligning itself #define SPEEDO 65 //SPEED is normal run speed #define SPEED 140 #define RAMMING_SPEED 150 //Flywheel #define FLYWHEEL_SPEED 160 //153 #define MAXD 200 #define NUM_SAMPLES 1 #define ACCEPTABLE_POSITIONING_ERROR 3 //5 seconds in safe space #define TIME_IN_SAFE_SPACE (5*SECOND) #define FIND_WEST_WALL_DIST 15 #define ZERO 0 #define LAUNCHING_TIME (SECOND) #define ORIENT_TIME SECOND/3 #define FLYWHEEL_WARMUP_TIME (2*SECOND) #define TIME_BETWEEN_ECHOES 30 #define WNQCALIBRATIONCONSTANT -2 #define SWQCALIBRATIONCONSTANT 0 #define OPEN_HOPPER_DELAY 130 #define CLOSE_HOPPER_DELAY 1000 #define SHOTS_NUM 11 /*----------Positional Constant Definitions---------------*/ //The below constants were obtained through testing; we calibrated the robot //so we would know the desired X and Y positions for firing and finding the //pressure sensor. #define MIN_DISTANCE_FROM_WALL 7 //SAFE_SPACE (Also start) Position #define SAFE_SPACE_POSITION_X_DIST MIN_DISTANCE_FROM_WALL #define SAFE_SPACE_POSITION_Y_DIST 46 //18in, 46 cm //WEST_PRESSURE_SENSOR #define WEST_PRESSURE_SENSOR_X_DIST 46 //18in, 46 cm #define WEST_PRESSURE_SENSOR_Y_DIST 2 //WEST_FIRING_POSITION 56 X and 46 Y #define WEST_FIRING_POSITION_X_DIST 56 //18in, 46 cm #define WEST_FIRING_POSITION_Y_DIST 47 //36in, 92cm Note: Was 47 //MID_FIRING_POSITION #define MID_FIRING_POSITION_X_DIST 119 //42in, 107cm #define MID_FIRING_POSITION_Y_DIST 47 //36in, 92cm Note: Was 47 #define ANGLE_DELAY_BEFORE 160 #define ANGLE_DELAY_AFTER 100 /*----------Module Function Prototypes------*/ void SetupPins(void); int sampleSensor(NewPing ping, int numSamples); void FindWestWall(void); void OrientAngle(void); void OrientAngleFromSouthWall(void); void AlignWestToEast(int xGoal); void AlignNorthToSouth(int yGoal); void AlignNorthToSouthRamming (int yGoal); void OpenHopper(void); void FireBalls(void); void CloseHopper(void); void WaitInSafeSpaceSpecifiedTime(void); void MoveNorth(void); void MoveSouth(void); void MoveEast(void); void MoveWest(void); void TurnCW(void); void TurnCCW(void); void Stop(void); //Set up Ultrasonic Sensors //West-North,West-South,South-East,South-West //Distance to West wall NewPing WNQ(WNO, WNI, MAXD); NewPing WSQ(WSO, WSI, MAXD); //Distance to South wall NewPing SEQ(SEO, SEI, MAXD); NewPing SWQ(SWO, SWI, MAXD); //Servo Motor Servo ballRelease; /*----------State Definitions--------*/ typedef enum { STATE_SETUP, STATE_MOVE_TO_PRESSURE_SENSOR, STATE_MOVE_TO_FIRST_LAUNCH_POSITION, STATE_MOVE_TO_SECOND_LAUNCH_POSITION, STATE_LAUNCHING, STATE_RELOAD, STATE_RETURN_TO_SAFE_SPACE } States_t; /*----------Module Variables---------*/ States_t state; int incomingByte = 0; int xCurrent = 0; int yCurrent = 0; int xGoal = 0; int yGoal = 0; int difference = 0; int sampleReadingUnique; //State Booleans int changeState = 0; int foundWestWall = 0; int angleOriented = 0; int lastFiringPositionWasWest = 1; int yPositionGood = 0; int xPositionGood = 0; int YCheckGood =0; int differenceCheck = 0; int SecondYpositionCheck = 0; //Servo Control int ServoPosition = 0; /*----------Main Functions------------*/ void setup() { Serial.begin(BAUD_RATE); SetupPins(); state = STATE_SETUP; } void loop() { switch (state) { case STATE_SETUP: //Start case needs to find which walls are which first // If the wall is already found, i.e., the robot is already //facing north, don't call FindWestWall // We could use either West sensor, but we choose WNQ sampleReadingUnique = sampleSensor(WNQ, NUM_SAMPLES); difference = sampleReadingUnique - FIND_WEST_WALL_DIST; //The following conditions all determine whether it still needs to //search for west wall //First condition to see if the wall is too far away to be West Wall //Second condition to make sure the sensor continues searching //for WestWall (ie facing opposite wall) //Third condition to make sure program remembers when it finds //the west wall and doesn't run FindWestWall again if (difference > ACCEPTABLE_POSITIONING_ERROR || sampleReadingUnique==0 && foundWestWall == 0) { FindWestWall(); Serial.println("Looking for West Wall"); } else { foundWestWall = 1; // Recalibrate angle to wall if (angleOriented == 0) OrientAngle(); if (angleOriented == 1) { //Determine where the robot is trying to go xGoal = SAFE_SPACE_POSITION_X_DIST; yGoal = SAFE_SPACE_POSITION_Y_DIST; //If the robot is not where it want to be, have it move that way if (yPositionGood == 0) AlignNorthToSouth(yGoal); if (xPositionGood == 0 && yPositionGood ==1) AlignWestToEast(xGoal); //If the robot is where it wants to be it's time to change state and reset //the position variables if (xPositionGood == 1 && yPositionGood ==1) changeState = 1; //Check conditions to see whether to move to another state if (changeState == 1){ state = STATE_MOVE_TO_PRESSURE_SENSOR; changeState = 0; xPositionGood = 0; yPositionGood = 0; angleOriented = 0; } } } break; case STATE_MOVE_TO_PRESSURE_SENSOR: // Recalibrate angle to wall if (angleOriented == 0){ OrientAngle();} if (angleOriented ==1){ xGoal = WEST_PRESSURE_SENSOR_X_DIST; yGoal = WEST_PRESSURE_SENSOR_Y_DIST; if (yPositionGood == 0) {AlignNorthToSouth(yGoal);} if (xPositionGood == 0 && yPositionGood ==1) AlignWestToEast(xGoal); if (xPositionGood == 1 && yPositionGood ==1) {changeState = 1;} //Check conditions to see whether to move to another state if (changeState == 1){ state = STATE_MOVE_TO_FIRST_LAUNCH_POSITION; //Reset sub-state booleans changeState = 0; xPositionGood = 0; yPositionGood = 0; angleOriented = 0; } } break; case STATE_MOVE_TO_FIRST_LAUNCH_POSITION: // Recalibrate angle to wall //angleOriented =1; if (angleOriented == 0) OrientAngle(); if (angleOriented ==1){ xGoal = WEST_FIRING_POSITION_X_DIST; yGoal = WEST_FIRING_POSITION_Y_DIST; if (yPositionGood == 0){ AlignNorthToSouth(yGoal) ; } if (yPositionGood == 1 && xPositionGood ==0){ AlignWestToEast(xGoal); } //Note: y alignment gets off again => Recheck SecondYpositionCheck = sampleSensor(SWQ, NUM_SAMPLES); differenceCheck = SecondYpositionCheck - yGoal; if (xPositionGood ==1 && yPositionGood ==1) { if (abs(differenceCheck) > ACCEPTABLE_POSITIONING_ERROR){ AlignNorthToSouth(yGoal); } else { YCheckGood = 1; } } if (xPositionGood == 1 && yPositionGood ==1 && YCheckGood ==1) { changeState = 1; } //Check conditions to see whether to move to another state if (changeState == 1){ state = STATE_LAUNCHING; changeState = 0; xPositionGood = 0; yPositionGood = 0; lastFiringPositionWasWest = 1; angleOriented = 0; YCheckGood = 0; } } break; case STATE_MOVE_TO_SECOND_LAUNCH_POSITION: // Recalibrate angle to wall if (angleOriented == 0) OrientAngle(); //angleOriented = 1; if (angleOriented == 1 && xPositionGood != xGoal/2){ xGoal = MID_FIRING_POSITION_X_DIST; yGoal = MID_FIRING_POSITION_Y_DIST; if (yPositionGood == 0) AlignNorthToSouth(yGoal); if (yPositionGood == 1 && xPositionGood ==0) { AlignWestToEast(xGoal); } //Note: y alignment gets off again => Recheck SecondYpositionCheck = sampleSensor(SWQ, NUM_SAMPLES); differenceCheck = SecondYpositionCheck - yGoal; if (xPositionGood ==1 && yPositionGood ==1) { if (abs(differenceCheck) > ACCEPTABLE_POSITIONING_ERROR){ AlignNorthToSouth(yGoal); } else { YCheckGood = 1; } } if (xPositionGood == 1 && yPositionGood ==1 && YCheckGood == 1) { changeState = 1; } if (changeState == 1){ state = STATE_LAUNCHING; YCheckGood =0; changeState = 0; xPositionGood = 0; yPositionGood = 0; angleOriented = 0; lastFiringPositionWasWest = 0; } } break; case STATE_LAUNCHING: if (angleOriented == 0 ){ OrientAngleS(); } if (angleOriented ==1) { yGoal = MID_FIRING_POSITION_Y_DIST; if (yPositionGood == 0) {AlignNorthToSouth(yGoal);} // Recalibrate angle to wall if (angleOriented == 1 && yPositionGood ==1){ Stop(); StartFlywheel(); delay(FLYWHEEL_WARMUP_TIME); OpenHopper(); //Keep hopper open for 0.13 sec and then close hopper again to //only allow one ball through for(int i = 0; i < SHOTS_NUM; i++){ OrientAngle(); OpenHopper(); delay(OPEN_HOPPER_DELAY); CloseHopper(); delay(CLOSE_HOPPER_DELAY); } CloseHopper(); StopFlywheel(); angleOriented = 0; xPositionGood = 0; yPositionGood = 0; state = STATE_RETURN_TO_SAFE_SPACE; } } break; //At this point the robot has launched all its nerf balls at the first launch //position and returns to the safe space to reload. case STATE_RELOAD: Serial.println("STATE_RELOAD"); yGoal = SAFE_SPACE_POSITION_Y_DIST; if (yPositionGood == 0) AlignNorthToSouth(yGoal); if (yPositionGood ==1){ WaitInSafeSpaceSpecifiedTime(); //Check which firing position was last engaged, and act accordingly if (lastFiringPositionWasWest == 0){ state = STATE_MOVE_TO_FIRST_LAUNCH_POSITION; yPositionGood =0; } else{ state = STATE_MOVE_TO_SECOND_LAUNCH_POSITION; yPositionGood =0; } } break; case STATE_RETURN_TO_SAFE_SPACE: // Recalibrate angle to wall if (angleOriented == 0) {OrientAngle();} if (angleOriented ==1 && xPositionGood != xGoal/2) { xGoal = SAFE_SPACE_POSITION_X_DIST; yGoal = SAFE_SPACE_POSITION_Y_DIST; //Check Y position, then both Y position and X position, aligning accordingly if (yPositionGood == 0){ AlignNorthToSouth(yGoal); Serial.println("Returning from Second Launch, Y positioning"); } if (yPositionGood == 1 && xPositionGood ==0){ Serial.println("Returning from Second Launch, X positioning"); AlignWestToEast(xGoal); } if (xPositionGood == 1 && yPositionGood ==1) {changeState = 1;} if (changeState == 1){ state = STATE_RELOAD; changeState = 0; xPositionGood = 0; yPositionGood = 0; angleOriented = 0; } } break; default: Serial.println("You should never get here."); } } /*----------SetUpPins Function Definition---------------*/ void SetupPins() { //Moving motors pinMode(NR, OUTPUT); pinMode(SR, OUTPUT); pinMode(WB, OUTPUT); pinMode(ER, OUTPUT); pinMode(NB, OUTPUT); pinMode(SB, OUTPUT); pinMode(WR, OUTPUT); pinMode(EB, OUTPUT); //Servo motor ballRelease.attach(SERVO_PIN); CloseHopper(); //Flywheel motor //Gate is high => Closed => Flywheel not spinning pinMode(FLYWHEEL_PIN_HIGH, OUTPUT); pinMode(FLYWHEEL_PIN_LOW, OUTPUT); StopFlywheel(); } /*----------Common Navigation Function Definition---------------*/ //Rotates the robot until the west sensors detect the west wall is within //an appropriate distance //(this above condition is currently in state function as a precondition //for calling this function) void FindWestWall(){ TurnCW(SPEEDO); } void OrientAngle(){ //Edgecase: Ignore if the sensors are reading zero if (sampleSensor(WNQ, NUM_SAMPLES) ==0 || sampleSensor(WSQ, NUM_SAMPLES) ==0) { return; } int sampleSensorWNQ = sampleSensor(WNQ, NUM_SAMPLES); int sampleSensorWSQ = sampleSensor(WSQ, NUM_SAMPLES); //If it's too far CW if (sampleSensorWNQ == sampleSensorWSQ){ Stop(); angleOriented = 1; return; } if (sampleSensorWNQ > sampleSensorWSQ){ TurnCCW(SPEEDO); //If it's too far CCW }else if (sampleSensorWNQ < sampleSensorWSQ){ TurnCW(SPEEDO); } else { angleOriented = 1; } delay(ANGLE_DELAY_BEFORE); Stop(); delay(ANGLE_DELAY_AFTER); } void OrientAngleS(){ //Edgecase: Ignore if If the sensors are reading zero if (sampleSensor(SWQ, NUM_SAMPLES) ==0 || sampleSensor(WSQ, NUM_SAMPLES) ==0) { return; } int sampleSensorSWQ = sampleSensor(SWQ, NUM_SAMPLES); int sampleSensorSEQ = sampleSensor(SEQ, NUM_SAMPLES); if (sampleSensorSEQ == sampleSensorSWQ){ Stop(); angleOriented = 1; return; } //If it's too far CW if (sampleSensorSWQ > sampleSensorSEQ){ TurnCCW(SPEEDO); //If it's too far CW }else if (sampleSensorSWQ < sampleSensorSEQ){ TurnCW(SPEEDO); } else{ angleOriented = 1; } delay(ANGLE_DELAY_BEFORE); Stop(); delay(ANGLE_DELAY_AFTER); } //This function accepts a yGoal position and moves the robot north or //south depending on whether the position is within an acceptable //error of the goal position void AlignNorthToSouth(int yGoal) { //Note: We have the choice of either of the south sensors (SWQ,SEQ). //I chose SWQ. //Distance to SouthWall yCurrent = sampleSensor(SWQ, 1); difference = yCurrent - yGoal; if(yCurrent > 100){ return; } //Check if in correct y position if (abs(difference) < ACCEPTABLE_POSITIONING_ERROR){ Stop(); yPositionGood = 1; } else { //If too far North, move South if (difference >= 0 ) { MoveSouth(SPEED); } if(difference <0) { MoveNorth(SPEED); } } } void AlignWestToEast(int xGoal){ //Note: We have the choice of either of the west sensors (WNQ,WSQ). //I chose WNQ. //Distance to SouthWall xCurrent = sampleSensor(WNQ, NUM_SAMPLES); difference = xCurrent - xGoal; //Check if in correct x position if (abs(difference) < ACCEPTABLE_POSITIONING_ERROR){ Stop(); xPositionGood = 1; } else { //If not in Correct x Position //If too far East, move West if (difference >= 0 ){ MoveWest(SPEED); } else { MoveEast(SPEED); } } } void AlignNorthToSouthRamming(int yGoal){ //Note: We have the choice of either of the south sensors (SWQ,SEQ). //I chose SWQ. //Distance to SouthWall yCurrent = sampleSensor(SWQ, NUM_SAMPLES); difference = yCurrent - yGoal; //Check if in correct y position if (abs(difference) < ACCEPTABLE_POSITIONING_ERROR){ Stop(); yPositionGood = 1; } else { //If not in Correct y Position //If too far North, move South if (difference >= 0 ){ MoveSouth(RAMMING_SPEED); } else { MoveNorth(RAMMING_SPEED); } } } /*----------Launch State Functions---------------*/ void StartFlywheel(){ analogWrite(FLYWHEEL_PIN_HIGH, FLYWHEEL_SPEED); analogWrite(FLYWHEEL_PIN_LOW, ZERO); } void CloseHopper(){ //Rotates servo motor 30 degrees to close hopper for (ServoPosition = 0; ServoPosition <SERVO_GATE_ANGLE_CHANGE; ServoPosition +=1){ ballRelease.write(ServoPosition); } } void OpenHopper(){ //Rotates servo motor 30 degrees from closed state to open hopper for (ServoPosition = SERVO_GATE_ANGLE_CHANGE;ServoPosition >= 1; ServoPosition -=1){ ballRelease.write(ServoPosition); } } void StopFlywheel(){ analogWrite(FLYWHEEL_PIN_HIGH, ZERO); analogWrite(FLYWHEEL_PIN_LOW, ZERO); } /*----------Safe Space Function Definition---------------*/ void WaitInSafeSpaceSpecifiedTime(){ delay(TIME_IN_SAFE_SPACE); } /*----------Get Distance Value Sample Function Definition---------------*/ //Take specified number of samples at sensor and average them //Note: Delay so that we don't take samples too close together. //Samples taken less than 30ms apart are invalid. int sampleSensor(NewPing pinger, int numSamples) { int count = 0; for (int i = 0; i < numSamples; i++) { count += pinger.ping_cm(); delay(TIME_BETWEEN_ECHOES); } int returnVal = (count / numSamples); return returnVal; } /*----------Motor Command Function Definitions---------------*/ //North,South,East,West,CW,CCW, and Stop void MoveNorth(int VAL) { analogWrite(ER, VAL); analogWrite(EB, ZERO); analogWrite(WR, VAL); analogWrite(WB, ZERO); } void MoveSouth(int VAL) { analogWrite(ER, ZERO); analogWrite(EB, VAL); analogWrite(WR, ZERO); analogWrite(WB, VAL); } void MoveWest(int VAL) { analogWrite(NR, ZERO); analogWrite(NB, VAL); analogWrite(SR, VAL); analogWrite(SB, ZERO); } void MoveEast(int VAL) { analogWrite(NR, VAL); analogWrite(NB, ZERO); analogWrite(SR, ZERO); analogWrite(SB, VAL); } void TurnCW(int VAL) { analogWrite(NR, VAL); analogWrite(NB, ZERO); analogWrite(SR, VAL); analogWrite(SB, ZERO); analogWrite(ER, ZERO); analogWrite(EB, VAL); analogWrite(WR, VAL); analogWrite(WB, ZERO); } void TurnCCW(int VAL) { analogWrite(NR, ZERO); analogWrite(NB, VAL); analogWrite(SR, ZERO); analogWrite(SB, VAL); analogWrite(ER, VAL); analogWrite(EB, ZERO); analogWrite(WR, ZERO); analogWrite(WB, VAL); } void Stop() { analogWrite(NR, ZERO); analogWrite(NB, ZERO); analogWrite(SR, ZERO); analogWrite(SB, ZERO); analogWrite(ER, ZERO); analogWrite(EB, ZERO); analogWrite(WR, ZERO); analogWrite(WB, ZERO); }