private static void SetMotorSpeed(TankRobot robot, WiiChuck wiiChuck)
{
var y = (int)(wiiChuck.AnalogY * 100);
if (System.Math.Abs(y) < 10)
{
y = 0;
}
var x = (int)(wiiChuck.AnalogX * 100);
if (System.Math.Abs(x) < 10)
{
x = 0;
}
var leftSpeed = y;
var rightSpeed = y;
leftSpeed += (int)(x * (1 - (y / 200f)));
rightSpeed -= (int)(x * (1 - (y / 200f)));
if (leftSpeed > 100)
{
leftSpeed = 100;
}
if (rightSpeed > 100)
{
rightSpeed = 100;
}
if (x != lastX || y != lastY)
//if (leftSpeed != _lastDataPoint.LeftSpeed || rightSpeed != _lastDataPoint.RightSpeed)
{
Debug.Print("X,Y = " + x + "," + y + " \t L,R = " + leftSpeed + "," + rightSpeed);
lastX = x;
lastY = y;
robot.Move(leftSpeed, rightSpeed);
if (Recording)
{
RecordData(leftSpeed, rightSpeed);
}
}
}
//public static void Main()
//{
// using (var a1 = new AnalogInput(Cpu.AnalogChannel.ANALOG_1))
// {
// var last = 0.0d;
// while (Debugger.IsAttached)
// {
// var reading = a1.Read();
// if (reading != last)
// {
// Debug.Print(reading.ToString());
// last = reading;
// }
// }
// }
//}
public static void Main()
{
var wiiChuck = new WiiChuck(true);
var debugMode = Debugger.IsAttached;
_redLed = new OutputPort(Pins.GPIO_PIN_D13, false);
_greenLed = new OutputPort(Pins.GPIO_PIN_A0, false);
_pot = new AnalogInput(Cpu.AnalogChannel.ANALOG_1);
//_speaker = new PWM(Pins.GPIO_PIN_D9);
//_speakerThread = new Thread(PlaySound);
//_speakerThread.Start();
//_servo1 = new ServoController(Mshield.Servo1, 600, 2400, startDegree: 90);
_robot = new TankRobot();
while (!debugMode || Debugger.IsAttached)
{
// try to read the data from nunchucku
if (wiiChuck.GetData())
{
CheckButtons(wiiChuck.CButtonDown, wiiChuck.ZButtonDown);
if (Recording)
{
_iteration++;
SetMotorSpeed(_robot, wiiChuck);
}
else if (PlayingBack)
{
if (_currentPlaybackIndex >= _record.Count)
{
PlayingBack = false;
_currentPlaybackIndex = 0;
}
else
{
_iteration++;
var record = (DataPoint) _record[_currentPlaybackIndex];
if (record.Iterations == _iteration)
{
_currentPlaybackIndex++;
_robot.Move(record.LeftSpeed, record.RightSpeed);
}
}
}
else
{
SetMotorSpeed(_robot, wiiChuck);
}
//var degrees = ((int) (90+(-90*wiiChuck.AccelerationXGs))/2)*2;
//if (degrees < 0)
// degrees = 0;
//else if (degrees > 180)
// degrees = 180;
//_servo1.Rotate(degrees);
//Debug.Print("AccelX = " + wiiChuck.AccelerationXGs + " AccelY=" + wiiChuck.AccelerationYGs);
}
}
wiiChuck.Dispose();
_robot.Dispose();
//_speaker.Dispose();
//_speaker = null;
//_servo1.Dispose();
_redLed.Dispose();
_greenLed.Dispose();
_pot.Dispose();
}
private static void SetMotorSpeed(TankRobot robot, WiiChuck wiiChuck)
{
var y = (int)(wiiChuck.AnalogY * 100);
if (System.Math.Abs(y) < 10)
y = 0;
var x = (int)(wiiChuck.AnalogX * 100);
if (System.Math.Abs(x) < 10)
x = 0;
var leftSpeed = y;
var rightSpeed = y;
leftSpeed += (int)(x * (1 - (y / 200f)));
rightSpeed -= (int)(x * (1 - (y / 200f)));
if (leftSpeed > 100)
leftSpeed = 100;
if (rightSpeed > 100)
rightSpeed = 100;
if (x != lastX || y != lastY)
//if (leftSpeed != _lastDataPoint.LeftSpeed || rightSpeed != _lastDataPoint.RightSpeed)
{
Debug.Print("X,Y = " + x + "," + y + " \t L,R = " + leftSpeed + "," + rightSpeed);
lastX = x;
lastY = y;
robot.Move(leftSpeed, rightSpeed);
if (Recording)
RecordData(leftSpeed, rightSpeed);
}
}
//public static void Main()
//{
// using (var a1 = new AnalogInput(Cpu.AnalogChannel.ANALOG_1))
// {
// var last = 0.0d;
// while (Debugger.IsAttached)
// {
// var reading = a1.Read();
// if (reading != last)
// {
// Debug.Print(reading.ToString());
// last = reading;
// }
// }
// }
//}
public static void Main()
{
var wiiChuck = new WiiChuck(true);
var debugMode = Debugger.IsAttached;
_redLed = new OutputPort(Pins.GPIO_PIN_D13, false);
_greenLed = new OutputPort(Pins.GPIO_PIN_A0, false);
_pot = new AnalogInput(Cpu.AnalogChannel.ANALOG_1);
//_speaker = new PWM(Pins.GPIO_PIN_D9);
//_speakerThread = new Thread(PlaySound);
//_speakerThread.Start();
//_servo1 = new ServoController(Mshield.Servo1, 600, 2400, startDegree: 90);
_robot = new TankRobot();
while (!debugMode || Debugger.IsAttached)
{
// try to read the data from nunchucku
if (wiiChuck.GetData())
{
CheckButtons(wiiChuck.CButtonDown, wiiChuck.ZButtonDown);
if (Recording)
{
_iteration++;
SetMotorSpeed(_robot, wiiChuck);
}
else if (PlayingBack)
{
if (_currentPlaybackIndex >= _record.Count)
{
PlayingBack = false;
_currentPlaybackIndex = 0;
}
else
{
_iteration++;
var record = (DataPoint)_record[_currentPlaybackIndex];
if (record.Iterations == _iteration)
{
_currentPlaybackIndex++;
_robot.Move(record.LeftSpeed, record.RightSpeed);
}
}
}
else
{
SetMotorSpeed(_robot, wiiChuck);
}
//var degrees = ((int) (90+(-90*wiiChuck.AccelerationXGs))/2)*2;
//if (degrees < 0)
// degrees = 0;
//else if (degrees > 180)
// degrees = 180;
//_servo1.Rotate(degrees);
//Debug.Print("AccelX = " + wiiChuck.AccelerationXGs + " AccelY=" + wiiChuck.AccelerationYGs);
}
}
wiiChuck.Dispose();
_robot.Dispose();
//_speaker.Dispose();
//_speaker = null;
//_servo1.Dispose();
_redLed.Dispose();
_greenLed.Dispose();
_pot.Dispose();
}
