 int16_t targetDir_r_field;
 uint8_t targetVelocity;
 
-/**********************************************************/
-/*				         Phasing   						  */
-/**********************************************************/
+bool inFrontOrbitMode = false;
+
+/**********************************************************/
+float dirBoundaries[2] = {-180, 180};
 
 
 /**********************************************************/

	}
}

void getMovement(uint8_t playMode){
	if (playMode == OFFENSE){
		if (linelocation == LINELOCATION::FIELD){
			targetVelocity = NORMAL_SPEED;

			orbit_k = 1;

			if (tsopStrength > 148){
				orbit_k = 1.0;
			}
			else if (tsopStrength > 140){
				orbit_k = 0.9;
			}
			else if (tsopStrength > 135){
				orbit_k = 0.8;
			}
			else if (tsopStrength > 130){
				orbit_k = 0.7;
			}
			else if (tsopStrength > 120){
				orbit_k = 0.6;
			}
			else if (tsopStrength > 100){
				targetVelocity = SUPER_SPEED;
				orbit_k = 0.55;
			}
			else if (tsopStrength > 70){
				targetVelocity = SUPER_SPEED;
				orbit_k = 0.5;
			}
			else{
				targetVelocity = 0;
			}

			if ((tsopAngle_r_targetBearing < -170 && tsopAngle_r_targetBearing >= -180)
			  || tsopAngle_r_targetBearing > 170 && tsopAngle_r_targetBearing <= 180){
				//
			}

			targetDir_r_field = (270 - abs(tsopAngle_r_targetBearing * orbit_k)) / 90 * tsopAngle_r_targetBearing * orbit_k;
			TOBEARING180(targetDir_r_field);
			if (targetDir_r_field < -170 || targetDir_r_field > 170){
				targetVelocity = SUPER_SPEED;
			}
		}
		else if (linelocation == LINELOCATION::SIDE_RIGHT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = -90;
		}
		else if (linelocation == LINELOCATION::SIDE_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 90;
		}
		else if (linelocation == LINELOCATION::CORNER_TOP_RIGHT || linelocation == LINELOCATION::CORNER_TOP_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 180;
		}
		else if (linelocation == LINELOCATION::CORNER_BOTTOM_RIGHT || linelocation == LINELOCATION::CORNER_BOTTOM_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 0;
		}
		else if (linelocation == LINELOCATION::UNKNOWN){
			targetVelocity = NORMAL_SPEED;
			// use ultrasonics now
			if (leftDistance <= 30 && rightDistance > 30){
				// left
				targetDir_r_field = 90;
			}
			else if (leftDistance > 30 && rightDistance <= 30){
				// right
				targetDir_r_field = -90;
			}
			else if (leftDistance <= 30 && rightDistance <= 30){
				// top or bottom
				if (backDistance <= 25){
					// bottom
					targetDir_r_field = 0;
				}
				else{
					// top
					targetDir_r_field = 180;
				}
			}
			else{
				// we're screwed
				targetVelocity = 0;
				targetDir_r_field = 0;
			}
		}
	}
	else if (playMode == DEFENSE){
		if (linelocation == LINELOCATION::FIELD){
			// check ultrasonics work
			int forwardV = 0, sideV = 0;
			
			if (leftDistance != 255 && rightDistance != 255){
				sideV = 2*(rightDistance - leftDistance);
			}
			if (backDistance != 255){
				if (backDistance > 30){
					forwardV = -100;
				}
				else if (backDistance < 15){
					forwardV = 100;
				}
			}
			targetDir_r_field = (int16_t)(atan2(sideV, forwardV)*180/PI);
			targetVelocity = 50;
		}
		else if (linelocation == LINELOCATION::SIDE_RIGHT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = -90;
		}
		else if (linelocation == LINELOCATION::SIDE_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 90;
		}
		else if (linelocation == LINELOCATION::CORNER_TOP_RIGHT || linelocation == LINELOCATION::CORNER_TOP_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 180;
		}
		else if (linelocation == LINELOCATION::CORNER_BOTTOM_RIGHT || linelocation == LINELOCATION::CORNER_BOTTOM_LEFT){
			targetVelocity = NORMAL_SPEED;
			targetDir_r_field = 0;
		}
		else if (linelocation == LINELOCATION::UNKNOWN){
			targetVelocity = 0;
			targetDir_r_field = 0;
			// // use ultrasonics now
			// if (leftDistance <= 30 && rightDistance > 30){
			// 	// left
			// }
			// else if (leftDistance > 30 && rightDistance <= 30){
			// 	// right
			// }
			// else if (leftDistance <= 30 && rightDistance <= 30){
			// 	// top or bottom
			// 	if (backDistance <= 30){
			// 		// bottom
			// 	}
			// 	else{
			// 		// top
			// 	}
			// }
			// else{
			// 	// we're screwed
			// }
			// linelocation = linelocation_prev;
		}
	}
	else{
		// ???

			}
		}

		// bearing boundaries
		switch (linelocation){
			case LINELOCATION::FIELD:
				dirBoundaries[0] = -180;
				dirBoundaries[1] = 180;
				break;
			case LINELOCATION::SIDE_LEFT:
				dirBoundaries[0] = -180;
				dirBoundaries[1] = 0;
				break;
			case LINELOCATION::SIDE_RIGHT:
				dirBoundaries[0] = 0;
				dirBoundaries[1] = 180;
				break;
			case LINELOCATION::CORNER_TOP_LEFT:
				dirBoundaries[0] = 90;
				dirBoundaries[1] = 180;
				break;
			case LINELOCATION::CORNER_TOP_RIGHT:
				dirBoundaries[0] = -180;
				dirBoundaries[1] = -90;
				break;
			case LINELOCATION::CORNER_BOTTOM_LEFT:
				dirBoundaries[0] = 0;
				dirBoundaries[1] = 90;
				break;
			case LINELOCATION::CORNER_BOTTOM_RIGHT:
				dirBoundaries[0] = -90;
				dirBoundaries[1] = 90;
				break;
			case LINELOCATION::SIDE_TOP:
				dirBoundaries[0] = 90;
				dirBoundaries[1] = -90;
				break;
			case LINELOCATION::SIDE_BOTTOM:
				dirBoundaries[0] = -90;
				dirBoundaries[1] = 90;
				break;
			case LINELOCATION::UNKNOWN:
				dirBoundaries[0] = -180;
				dirBoundaries[1] = 180;
				break;
		}

		/* tsops */
		getSlave2Data();
		tsopAngle_r_field = tsopAngle + bearing_int;
