public static nxt_result motor_degree(NXTBrick.Motor motor, int degree)
{
nxt_result result = new nxt_result();
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
if (motor == NXTBrick.Motor.A)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[0];
motorState.TurnRatio = (sbyte)speed_motors[0];
}
if (motor == NXTBrick.Motor.B)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[1];
motorState.TurnRatio = (sbyte)speed_motors[1];
}
if (motor == NXTBrick.Motor.C)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[2];
motorState.TurnRatio = (sbyte)speed_motors[2];
}
// TODO If al motor's on handel correct speed
if (motor == NXTBrick.Motor.All)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[0];
motorState.TurnRatio = (sbyte)speed_motors[0];
}
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Running;
// tacho limit
motorState.TachoLimit = degree;
// set motor's state
if (nxt.SetMotorState(motor, motorState) != true)
{
IrcBot.log += "Failed setting motor state\n";
result.result = false;
result.value = "Failed";
return result;
}
result.result = true;
result.value = "succed";
return result;
}
public static nxt_result motor_off(NXTBrick.Motor motor)
{
nxt_result result = new nxt_result();
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
// prepare motor's state to set
motorState.Power = (sbyte)0;
motorState.TurnRatio = (sbyte)0;
motorState.Mode = NXTBrick.MotorMode.None;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Idle;
// tacho limit
motorState.TachoLimit = 0;
// set motor's state
if (nxt.SetMotorState(motor, motorState) != true)
{
IrcBot.log += "Failed setting motor state\n";
result.result = false;
result.value = "Failed";
return result;
}
result.result = true;
result.value = "succed";
return result;
}
/// <summary>
/// StartCutting: Main Cutting Method. Before calling this method, be sure that the 3d machine is ready.
/// </summary>
/// <param name="calibrationOnly"></param>
private void StartCutting(bool calibrationOnly)
{
MillingMachineManager myMillingMachineManager = new MillingMachineManager(AppDomain.CurrentDomain.BaseDirectory + @"\head-vise.obj");
threeDMotorState = new NXTBrick.MotorState();
// prepare motor's state to set
threeDMotorState.Power = Convert.ToByte(powerUpDown.Text);
threeDMotorState.TurnRatio = 0;
threeDMotorState.Mode = ((modeOnCheck.IsChecked == true) ? NXTBrick.MotorMode.On : NXTBrick.MotorMode.None) |
((modeBrakeCheck.IsChecked == true) ? NXTBrick.MotorMode.Brake : NXTBrick.MotorMode.None) |
((modeRegulatedBox.IsChecked == true) ? NXTBrick.MotorMode.Regulated : NXTBrick.MotorMode.None);
threeDMotorState.Regulation = regulationModes[regulationModeCombo.SelectedIndex];
threeDMotorState.RunState = runStates[2];
// Prepare sensor state
if (nxt.SetSensorMode(GetSelectedSensor(),
sensorTypes[BLUE_COLOR_SENSOR_STATE_INDEX],
sensorModes[0], false) != true)
{
System.Diagnostics.Debug.WriteLine("Failed setting input mode");
}
List<Displacement> displacementList;
if (calibrationOnly)
{
displacementList = myMillingMachineManager.GetVertexCalibrationListForMachineCoordinatesAsDisplacement();
}
else
{
displacementList = myMillingMachineManager.GetVertexListForMachineCoordinatesAsDisplacement();
}
foreach (Displacement v in displacementList)
{
// Run X axis motor
int xDisplacement = Convert.ToInt32(v.x);
if (RunMotorInAnAxis(2, xDisplacement))
{
Thread.Sleep(Math.Abs(xDisplacement * 100));
}
//displayTxt.Text += xDisplacement + ",";
// Z
int zDisplacement = Convert.ToInt32(v.z);
if (RunMotorInAnAxis(1, zDisplacement))
{
Thread.Sleep(Math.Abs(zDisplacement * 100));
}
// This is the main rotating/cutting motor. We use actual vertex information rather than displacement information and
// use color detector for returning back
int yPosition;
if (calibrationOnly)
{
yPosition = Convert.ToInt32(v.y);
}
else
{
yPosition = Convert.ToInt32(v.TargetVertex.y);
}
if (RunMotorInAnAxis(0, yPosition))
{
Thread.Sleep(Math.Abs(yPosition * 10));
NXTBrick.SensorValues sensorValues;
do
{
// get input values
if (nxt.GetSensorValue(GetSelectedSensor(), out sensorValues))
{
validCheck.IsChecked = sensorValues.IsValid;
calibratedCheck.IsChecked = sensorValues.IsCalibrated;
sensorTypeBox.Text = sensorValues.SensorType.ToString();
sensorModeBox.Text = sensorValues.SensorMode.ToString();
rawInputBox.Text = sensorValues.Raw.ToString();
normalizedInputBox.Text = sensorValues.Normalized.ToString();
scaledInputBox.Text = sensorValues.Scaled.ToString();
calibratedInputBox.Text = sensorValues.Calibrated.ToString();
}
else
{
System.Diagnostics.Debug.WriteLine("Failed getting input values");
}
// Now go up until blue color is reached
threeDMotorState.Power = -1 * Convert.ToByte(powerUpDown.Text);
threeDMotorState.TachoLimit = 5;
// set motor's state
if (nxt.SetMotorState(GetSelectedMotorY(), threeDMotorState, false) != true)
{
System.Diagnostics.Debug.WriteLine("Failed setting motor state");
}
Thread.Sleep(Math.Abs(50));
} while (sensorValues.Raw < 250);
}
}
}
// On motor "Set state" button click
private void setMotorStateButton_Click( object sender, EventArgs e )
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState( );
// prepare motor's state to set
motorState.Power = (sbyte) powerUpDown.Value;
motorState.TurnRatio = (sbyte) turnRatioUpDown.Value;
motorState.Mode = ( ( modeOnCheck.Checked ) ? NXTBrick.MotorMode.On : NXTBrick.MotorMode.None ) |
( ( modeBrakeCheck.Checked ) ? NXTBrick.MotorMode.Brake : NXTBrick.MotorMode.None ) |
( ( modeRegulatedBox.Checked ) ? NXTBrick.MotorMode.Regulated : NXTBrick.MotorMode.None );
motorState.Regulation = regulationModes[regulationModeCombo.SelectedIndex];
motorState.RunState = runStates[runStateCombo.SelectedIndex];
// tacho limit
try
{
motorState.TachoLimit = Math.Max( 0, Math.Min( 100000, int.Parse( tachoLimitBox.Text ) ) );
}
catch
{
motorState.TachoLimit = 1000;
tachoLimitBox.Text = motorState.TachoLimit.ToString( );
}
// set motor's state
if ( nxt.SetMotorState( GetSelectedMotor( ), motorState, false ) != true )
{
System.Diagnostics.Debug.WriteLine( "Failed setting motor state" );
}
}
/**
* This will set the motor on
* @param motor type NXTBrick.Motor
* @return nxt_result
*/
public static nxt_result motor_on(NXTBrick.Motor motor, int degrees, int number)
{
//Debug.WriteLine("Set motor on");
//Debug.WriteLine(motor);
nxt_result result = new nxt_result();
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
if (motor == NXTBrick.Motor.A)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[0];
motorState.TurnRatio = 50;
}
if (motor == NXTBrick.Motor.B)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[1];
motorState.TurnRatio = 50;
}
if (motor == NXTBrick.Motor.C)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[2];
motorState.TurnRatio = 50;
}
// TODO If al motor's on handel correct speed
if (motor == NXTBrick.Motor.All)
{
// prepare motor's state to set
motorState.Power = (sbyte)speed_motors[0];
motorState.TurnRatio = 50;
}
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.RotationCount = 0;
// tacho limit
motorState.TachoCount = 0;
if (motorState.Power < 0)
{
motorState.TachoLimit = -degrees;
}
else
{
motorState.TachoLimit = degrees;
}
// set motor's state
if (nxt.SetMotorState(motor, motorState) != true)
{
IrcBot.log += "Failed setting motor state\n";
result.result = false;
result.value = "Failed";
return result;
}
result.result = true;
result.value = "succed";
return result;
}
private void RightForward()
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 70;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 360;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
}
private void OneStepRight(int tacho)
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 75;
motorState.TurnRatio = 100;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = tacho;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
}
private void MoveArm(int angle, int power, int turnRatio)
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = power;
motorState.TurnRatio = turnRatio;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = angle;
nxt.SetMotorState(NXTBrick.Motor.A, motorState);
}
private void LeftBackwards()
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = -70;
motorState.TurnRatio = -50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Speed;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 360;
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}