static void Main()
{
////////// Set these to match your board //////////////
var clickRstPin = SC20100.GpioPin.PD4;
var clickCsPin = SC20100.GpioPin.PD3;
var clickPwmPin = SC20100.PwmChannel.Controller2.PA15;
var clickPwmController = SC20100.PwmChannel.Controller2.Id;
//var ClickRstPin = SC20100.GpioPin.PD15;
//var ClickCsPin = SC20100.GpioPin.PD14;
//var ClickPwmPin = SC20100.PwmChannel.Controller13.PA6;
//var ClickPwmController = SC20100.PwmChannel.Controller13.Id;
///////////////////////////////////////////////////////
var gpio = GpioController.GetDefault();
var motor = new DCMotor(
gpio.OpenPin(clickRstPin),
gpio.OpenPin(clickCsPin),
PwmController.FromName(clickPwmController).OpenChannel(clickPwmPin));
while (true)
{
motor.Set(90); // Forward at 90% speed
Thread.Sleep(3000);
motor.Set(-70); // Reverse at 70% speed
Thread.Sleep(3000);
}
}
public static void Main()
{
DCMotor leftWheel = new DCMotor(MotorHeaders.M4);
DCMotor rightWheel = new DCMotor(MotorHeaders.M3);
leftWheel.SetSpeed(0);
rightWheel.SetSpeed(0);
leftWheel.Run(DCMotor.MotorDirection.Forward);
rightWheel.Run(DCMotor.MotorDirection.Forward);
leftWheel.SetSpeed(90);
rightWheel.SetSpeed(90);
while (true)
{
// spin left
leftWheel.Run(DCMotor.MotorDirection.Reverse);
rightWheel.Run(DCMotor.MotorDirection.Forward);
Thread.Sleep(1000);
// spin right
leftWheel.Run(DCMotor.MotorDirection.Forward);
rightWheel.Run(DCMotor.MotorDirection.Reverse);
Thread.Sleep(1000);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="DriveWheelMechanism"/> class.
/// </summary>
/// <param name="motor">The motor.</param>
/// <param name="input">The input.</param>
/// <param name="wheelbaseXM">The wheelbase xm.</param>
/// <param name="wheelDiameterM">The wheel diameter m.</param>
/// <param name="output">The output.</param>
/// <param name="wheelStaticCoef">The wheel static coef.</param>
/// <param name="wheelDynamicCoef">The wheel dynamic coef.</param>
/// <param name="staticFriction">The static friction.</param>
/// <param name="dynamicFriction">The dynamic friction.</param>
public DriveWheelMechanism(DCMotor motor, ISimSpeedController input, double wheelbaseXM, double wheelDiameterM, IServoFeedback output,
double wheelStaticCoef = 1.07, double wheelDynamicCoef = 0.1, double staticFriction = 0.1, double dynamicFriction = 0.01) :
this(motor, input, wheelbaseXM, wheelDiameterM, wheelStaticCoef, wheelDynamicCoef,
staticFriction, dynamicFriction)
{
m_output = output;
}
public static void MotorTest(List <string> argsList, DCMotor motorL, DCMotor motorR)
{
double delay;
if (argsList.Count > 1)
{
delay = Convert.ToDouble(argsList[1]);
}
else
{
delay = 10;
}
const double Period = 20.0;
Console.WriteLine($"Motor Test");
Stopwatch sw = Stopwatch.StartNew();
string lastSpeedDisp = null;
while (sw.ElapsedMilliseconds < (Math.PI * 2000))
{
double time = sw.ElapsedMilliseconds / 1000.0;
// Note: range is from -1 .. 1 (for 1 pin setup 0 .. 1)
motorL.Speed = Math.Sin(2.0 * Math.PI * time / Period);
motorR.Speed = Math.Sin(2.0 * Math.PI * time / Period);
string disp = $"Speed[L, R] = [{motorL.Speed:0.00}, {motorR.Speed:0.00}]";
if (disp != lastSpeedDisp)
{
lastSpeedDisp = disp;
Console.WriteLine(disp);
}
DelayHelper.DelayMilliseconds((int)delay, true);
}
}
public static void Main()
{
DCMotor leftWheel = new DCMotor(MotorHeaders.M4);
DCMotor rightWheel = new DCMotor(MotorHeaders.M3);
leftWheel.SetSpeed(0);
rightWheel.SetSpeed(0);
leftWheel.Run(DCMotor.MotorDirection.Forward);
rightWheel.Run(DCMotor.MotorDirection.Forward);
leftWheel.SetSpeed(90);
rightWheel.SetSpeed(90);
while (true)
{
// spin left
leftWheel.Run(DCMotor.MotorDirection.Reverse);
rightWheel.Run(DCMotor.MotorDirection.Forward);
Thread.Sleep(1000);
// spin right
leftWheel.Run(DCMotor.MotorDirection.Forward);
rightWheel.Run(DCMotor.MotorDirection.Reverse);
Thread.Sleep(1000);
}
}
public MotorController()
{
motorHat = new MotorHat();
leftmotor = motorHat.CreateDCMotor(1);
rightmotor = motorHat.CreateDCMotor(2);
}
/*
* Make a transmission.
*
* @param motors The motor type attached to the transmission.
* @param num_motors The number of motors in this transmission.
* @param gear_reduction The reduction of the transmission.
* @param efficiency The efficiency of the transmission.
* @return A DCMotor representing the combined transmission.
*/
public static DCMotor MakeTransmission(DCMotor motor, int num_motors,
double gear_reduction, double efficiency)
{
return(new DCMotor(motor.MaxTorque * num_motors * gear_reduction, motor.MaxSpeed / gear_reduction)
{
Efficiency = efficiency
});
}
/*
* Make a transmission.
*
* @param motors The motor type attached to the transmission.
* @param num_motors The number of motors in this transmission.
* @param gear_reduction The reduction of the transmission.
* @param efficiency The efficiency of the transmission.
* @return A DCMotor representing the combined transmission.
*/
public static DCMotor MakeTransmission(DCMotor motor, int num_motors,
double gear_reduction, double efficiency)
{
return new DCMotor(motor.MaxTorque*num_motors*gear_reduction, motor.MaxSpeed/gear_reduction)
{
Efficiency = efficiency
};
}
public MotorController(GpioController controller, DCMotor leftMotor, DCMotor rightMotor)
{
Contract.Requires(controller != null);
Contract.Requires(leftMotor != null);
Contract.Requires(rightMotor != null);
Controller = controller;
LeftMotor = leftMotor;
RightMotor = rightMotor;
}
public static void Add(string name, DCMotor type)
{
if (Types.ContainsKey(name))
{
throw new SynthesisException($"Cannot add new motor type with existing name {name}");
}
AllTypeNames.Add(name);
type.Name = name;
Types[name] = type;
}
private float GetTorque(WheelCollider wheel, DCMotor motor, float percentOut)
{
// torque = stallTorque * (percentVolts - percentVel)
percentOut = Mathf.Clamp(percentOut, -1, 1);
float percentVel = wheel.rpm / maxWheelSpeed;
float percentTorque = (percentOut - wheel.rpm / maxWheelSpeed);
float torque = percentTorque * motor.StallTorque / gearing;
Debug.Log("out: " + percentOut + ", vel: " + percentVel + ", torque: " + percentTorque);
return(torque);
}
public AngularPotentiometerMechanism(ISimSpeedController input, SimAnalogInput output, DCMotor model,
double startPercentage, double potentiometerRadians, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
m_maxRadians = potentiometerRadians;
m_minRadians = 0;
CurrentRadians = (m_maxRadians - m_minRadians) *startPercentage;
m_invert = invertInput;
m_scaler = 5 / (m_maxRadians - m_minRadians);
}
/// <summary>
/// Initializes a new instance of the <see cref="AngularPotentiometerMechanism"/> class.
/// </summary>
/// <param name="input">The motor driving the system</param>
/// <param name="output">The Analog Input giving feedback to the system..</param>
/// <param name="model">The motor model.</param>
/// <param name="startPercentage">The starting percentage of the potentiometer from 0.</param>
/// <param name="potentiometerRotations">The number of rotations the potentiometer has.</param>
/// <param name="invertInput">if set to <c>true</c> [invert input].</param>
public AngularPotentiometerMechanism(ISimSpeedController input, SimAnalogInput output, DCMotor model,
double startPercentage, double potentiometerRotations, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
m_maxRadians = potentiometerRotations * (Math.PI * 2);
m_minRadians = 0;
CurrentRadians = (m_maxRadians - m_minRadians) * startPercentage;
m_invert = invertInput;
m_scaler = 5 / (m_maxRadians - m_minRadians);
}
/// <summary>
/// Initializes a new instance of the <see cref="DriveWheelMechanism"/> class.
/// </summary>
/// <param name="motor">The motor.</param>
/// <param name="input">The input.</param>
/// <param name="wheelbaseXM">The wheelbase xm.</param>
/// <param name="wheelDiameterM">The wheel diameter m.</param>
/// <param name="wheelStaticCoef">The wheel static coef.</param>
/// <param name="wheelDynamicCoef">The wheel dynamic coef.</param>
/// <param name="staticFriction">The static friction.</param>
/// <param name="dynamicFriction">The dynamic friction.</param>
public DriveWheelMechanism(DCMotor motor, ISimSpeedController input, double wheelbaseXM, double wheelDiameterM,
double wheelStaticCoef = 1.07, double wheelDynamicCoef = 0.1, double staticFriction = 0.1, double dynamicFriction = 0.01)
{
m_model = motor;
m_output = null;
m_input = input;
XPosition = -wheelbaseXM;
StaticCoef = wheelStaticCoef;
DynamicCoef = wheelDynamicCoef;
WheelDiameter = wheelDiameterM;
StaticFriction = staticFriction;
DynamicFriction = dynamicFriction;
}
static void Main(string[] args)
{
const double Period = 10.0;
DCMotorSettings settings = ThreePinMode();
Stopwatch sw = Stopwatch.StartNew();
using (DCMotor motor = DCMotor.Create(settings))
{
double time = sw.ElapsedMilliseconds / 1000.0;
// Note: range is from -1 .. 1 (for 1 pin setup 0 .. 1)
motor.Speed = Math.Sin(2.0 * Math.PI * time / Period);
}
}
public void TestSpinUpTime()
{
using (Talon t = new Talon(0))
using (Counter c = new Counter(0))
{
ISimSpeedController s = new SimPWMController(0);
//IServoFeedback f = new SimCounter(0);
DCMotor motor = DCMotor.MakeCIM();
double inertia = 0.005;
double deaccel = -80.0;
}
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.Escape))
{
dcmotor.VelocityUpdate -= onVelocityUpdate;
dcmotor.Close();
dcmotor = null;
vol.VoltageRatioChange -= onVoltageRatioChange;
vol.Close();
vol = null;
Application.Quit();
}
}
public DCMotor GetMotor(int index)
{
if (index > 4)
{
return(null);
}
index--;
if (null == MotorList[index])
{
MotorList[index] = new DCMotor(this, index);
}
return(MotorList[index]);
}
/// <summary>
/// Initializes a new instance of the <see cref="ShooterWheelMechanism"/> class.
/// </summary>
/// <param name="input">The speed controller being used.</param>
/// <param name="output">The feedback output.</param>
/// <param name="model">The motor model.</param>
/// <param name="invertInput">True if the input is inverted.</param>
/// <param name="minimumVelocity">The minimum velocity in Rotations Per Minute</param>
/// <param name="deaccelConstant">The deaccel constant in Rotations Per Minute</param>
/// <param name="systemInertia">The system inertia in kg*m^2.</param>
/// <param name="cpr">The counts per rotation of the sensor.</param>
/// <exception cref="ArgumentOutOfRangeException">Shooter Wheels do not support analog inputs</exception>
public ShooterWheelMechanism(ISimSpeedController input, IServoFeedback output, DCMotor model,
bool invertInput, double minimumVelocity, double deaccelConstant, double systemInertia, double cpr)
{
if (output is SimAnalogInput)
{
throw new ArgumentOutOfRangeException(nameof(output), "Shooter Wheels do not support analog inputs");
}
m_input = input;
m_output = output;
m_model = model;
m_invert = invertInput;
m_minimumVelocity = minimumVelocity.RpmsToRadiansPerSecond();
DeaccelerationConstant = deaccelConstant.RpmsToRadiansPerSecond();
SystemInertia = systemInertia;
m_cpr = cpr;
}
/// <summary>
/// Creates a new arm which is driven by an encoder
/// </summary>
/// <param name="input">The motor driving the system</param>
/// <param name="output">The encoder giving feedback to the system</param>
/// <param name="encoderCPR">The CPR of the encoder. If not a 1:1 ratio on the axle, scale this beforehand</param>
/// <param name="model">The transmission model to use</param>
/// <param name="startRotations">The location in rotations you want the system to start at.</param>
/// <param name="invertInput">Inverts the motor input</param>
public AngularEncoderMechanism(ISimSpeedController input, SimEncoder output, double encoderCPR, DCMotor model,
double startRotations, bool invertInput)
{
m_input = input;
//m_output = output;
m_model = model;
m_invert = invertInput;
m_scaler = encoderCPR / (Math.PI * 2);
CurrentRadians = 0;
m_offset = startRotations * (Math.PI * 2);
AdjustedRadians = m_offset;
m_maxRadians = double.MaxValue;
m_minRadians = double.MinValue;
}
/// <summary>
/// Initializes a new instance of the <see cref="LinearEncoderMechanism"/> class.
/// </summary>
/// <param name="input">The motor driving the system.</param>
/// <param name="output">The encoder giving feedback to the system.</param>
/// <param name="encoderCpr">The CPR of the encoder. If not a 1:1 ratio on the axle, scale this beforehand.</param>
/// <param name="model">The transmission model to use.</param>
/// <param name="spoolRadius">The radius of your spool in Meters. (Use the radius of the up spool if using a cascade elevator).</param>
/// <param name="startHeight">The start height of your elevator relative to the reset sensor in meters. If no reset sensor then use 0.</param>
/// <param name="invertInput">if set to <c>true</c> [invert input].</param>
public LinearEncoderMechanism(ISimSpeedController input, SimEncoder output, double encoderCpr, DCMotor model,
double spoolRadius, double startHeight, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
m_invert = invertInput;
RadiansPerMeter = 1 / spoolRadius;
m_scaler = encoderCpr / (Math.PI * 2);
m_offset = startHeight;
CurrentMeters = m_offset;
m_maxRadians = double.MaxValue;
m_minRadians = double.MinValue;
}
public static void Main(string[] args)
{
const double Period = 10.0;
Stopwatch sw = Stopwatch.StartNew();
// 1 pin mode
// using (DCMotor motor = DCMotor.Create(6))
// using (DCMotor motor = DCMotor.Create(PwmChannel.Create(0, 0, frequency: 50)))
// 2 pin mode
// using (DCMotor motor = DCMotor.Create(27, 22))
// using (DCMotor motor = DCMotor.Create(new SoftwarePwmChannel(27, frequency: 50), 22))
// 2 pin mode with BiDirectional Pin
// using (DCMotor.Create(19, 26, null, true, true))
// using (DCMotor.Create(PwmChannel.Create(0, 1, 100, 0.0), 26, null, true, true))
// 3 pin mode
// using (DCMotor motor = DCMotor.Create(PwmChannel.Create(0, 0, frequency: 50), 23, 24))
using (DCMotor motor = DCMotor.Create(6, 27, 22))
{
bool done = false;
Console.CancelKeyPress += (o, e) =>
{
done = true;
e.Cancel = true;
};
string lastSpeedDisp = null;
while (!done)
{
double time = sw.ElapsedMilliseconds / 1000.0;
// Note: range is from -1 .. 1 (for 1 pin setup 0 .. 1)
motor.Speed = Math.Sin(2.0 * Math.PI * time / Period);
string disp = $"Speed = {motor.Speed:0.00}";
if (disp != lastSpeedDisp)
{
lastSpeedDisp = disp;
Console.WriteLine(disp);
}
Thread.Sleep(1);
}
}
}
//String travel and Spool Radius in meters
public LinearPotentiometerMechanism(ISimSpeedController input, SimAnalogInput output, DCMotor model,
double startPercentage, double stringTravel, double spoolRadius, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
double metersPerRotation = Math.PI * (spoolRadius * 2);
double totalRotations = stringTravel / metersPerRotation;
double totalRadians = totalRotations * Math.PI * 2;
RadiansPerMeter = totalRadians / stringTravel;
m_maxRadians = totalRadians;
m_minRadians = 0;
CurrentRadians = (m_maxRadians - m_minRadians) * startPercentage;
CurrentMeters = CurrentRadians / RadiansPerMeter;
m_invert = invertInput;
m_scaler = 5/ (m_maxRadians - m_minRadians);
}
public LinearEncoderMechanism(ISimSpeedController input, SimEncoder output, double encoderCPR, DCMotor model,
double spoolRadius, double startHeight, bool invertInput)//, double topLimit = double.MaxValue / 500)
{
m_input = input;
m_output = output;
m_model = model;
m_invert = invertInput;
RadiansPerMeter = 1 / spoolRadius;
m_scaler = encoderCPR / (Math.PI * 2);
m_offset = startHeight;
CurrentMeters = m_offset;
m_maxRadians = double.MaxValue;
m_minRadians = double.MinValue;
}
//String travel and Spool Radius in meters
/// <summary>
/// Initializes a new instance of the <see cref="LinearPotentiometerMechanism"/> class.
/// </summary>
/// <param name="input">The motor driving the system.</param>
/// <param name="output">The potentiometer giving feedback to the system.</param>
/// <param name="model">The motor model with transmission to use.</param>
/// <param name="startPercentage">The starting percentages of the potentiometer from 0.</param>
/// <param name="stringTravel">The potentiometer travel scaled to be linear (in meters).</param>
/// <param name="spoolRadius">The radius of your spool in Meters. (Use the radius of the up spool if using a cascade elevator).</param>
/// <param name="invertInput">if set to <c>true</c> [invert input].</param>
public LinearPotentiometerMechanism(ISimSpeedController input, SimAnalogInput output, DCMotor model,
double startPercentage, double stringTravel, double spoolRadius, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
double metersPerRotation = Math.PI * (spoolRadius * 2);
double totalRotations = stringTravel / metersPerRotation;
double totalRadians = totalRotations * Math.PI * 2;
RadiansPerMeter = totalRadians / stringTravel;
m_maxRadians = totalRadians;
m_minRadians = 0;
CurrentRadians = (m_maxRadians - m_minRadians) * startPercentage;
CurrentMeters = CurrentRadians / RadiansPerMeter;
m_invert = invertInput;
m_scaler = 5 / (m_maxRadians - m_minRadians);
}
/// <summary>
/// Creates a new arm which is driven by an encoder
/// </summary>
/// <param name="input">The motor driving the system</param>
/// <param name="output">The encoder giving feedback to the system</param>
/// <param name="encoderCPR">The CPR of the encoder. If not a 1:1 ratio on the axle, scale this beforehand</param>
/// <param name="model">The transmission model to use</param>
/// <param name="startRadians">The location in radians you want the system to start at.</param>
/// <param name="invertInput">Inverts the motor input</param>
public AngularEncoderMechanism(ISimSpeedController input, SimEncoder output, double encoderCPR, DCMotor model,
double startRadians, bool invertInput)
{
m_input = input;
m_output = output;
m_model = model;
m_invert = invertInput;
m_scaler = encoderCPR / (Math.PI * 2);
CurrentRadians = 0;
m_offset = startRadians;
AdjustedRadians = m_offset;
m_maxRadians = double.MaxValue;
m_minRadians = double.MinValue;
}
public MotorService()
{
_motor1 = DCMotor.Create(GPIO.ENA, GPIO.IN1, GPIO.IN2, null, false);
_motor2 = DCMotor.Create(GPIO.ENB, GPIO.IN3, GPIO.IN4, null, false);
}
public DCMotorDriver(DCMotor motor, ILogger <DCMotorDriver> logger)
{
_dcMotor = motor;
_log = logger;
}
public MotorController(GpioController controller) : this(controller, DCMotor.Create(new SoftwarePwmChannel(AlphaBotPins.MotorSpeedA, frequency : 50), AlphaBotPins.MotorDirectionA1, AlphaBotPins.MotorDirectionA2), DCMotor.Create(new SoftwarePwmChannel(AlphaBotPins.MotorSpeedB, frequency : 50), AlphaBotPins.MotorDirectionB1, AlphaBotPins.MotorDirectionB2))
{
}
/// <summary>
/// Constructs instance with added Start() and Stop() as additional protection
/// </summary>
/// <param name="innerMotor">Crate DCMotor instance</param>
public DCMotorWithStartStop(DCMotor innerMotor)
: base(null, true)
{
_inner = innerMotor;
_speed = innerMotor.Speed;
}
public void Enable(Accessories accessory)
{
try
{
switch (accessory)
{
case Accessories.Camera:
if (camera is null)
{
this.camera = new Camera();
}
else
{
Disable(Accessories.Camera); this.camera = new Camera();
}
break;
case Accessories.IMU:
if (imu is null)
{
this.imu = new Mpu6050(I2cDevice.Create(
new I2cConnectionSettings(1, Mpu6050.DefaultI2cAddress)));
}
else
{
Disable(Accessories.IMU);
this.imu = new Mpu6050(I2cDevice.Create(
new I2cConnectionSettings(1, Mpu6050.DefaultI2cAddress)));
}
break;
case Accessories.MotorL:
if (motorL is null)
{
this.motorL = DCMotor.Create(6, 12, 13);
}
else
{
Disable(Accessories.MotorL); this.motorL = DCMotor.Create(6, 12, 13);
}
break;
case Accessories.MotorR:
if (motorR is null)
{
this.motorR = DCMotor.Create(26, 20, 21);
}
else
{
Disable(Accessories.MotorR); this.motorR = DCMotor.Create(26, 20, 21);
}
break;
case Accessories.Motors:
Enable(Accessories.MotorL);
Enable(Accessories.MotorR);
break;
case Accessories.ADC:
if (adc is null)
{
this.adc = new Tlc1543(24, 5, 23, 25);
}
else
{
Disable(Accessories.ADC); this.adc = new Tlc1543(24, 5, 23, 25);
}
break;
case Accessories.IR:
if (ir is null)
{
this.ir = new IrReceiver(17);
}
else
{
Disable(Accessories.IR); this.ir = new IrReceiver(17);
}
break;
case Accessories.Sonar:
if (sonar is null)
{
this.sonar = new Hcsr04(22, 27);
}
else
{
Disable(Accessories.Sonar); this.sonar = new Hcsr04(22, 27);
}
break;
case Accessories.LED:
if (led is null)
{
this.led = new Ws2812b(18, 4);
}
else
{
Disable(Accessories.LED); this.led = new Ws2812b(18, 4);
}
break;
case Accessories.CPUTemp:
if (cpuTemperature is null)
{
this.cpuTemperature = new CpuTemperature();
}
//else { Disable(led); this.led = new Ws2812b(18, 4); }
break;
case Accessories.All:
foreach (var item in Enum.GetValues(typeof(Accessories)))
{
Enable((Accessories)item);
}
break;
default:
Console.WriteLine("Something went wrong (Enabling accessories)");
break;
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Enabling accessory: {Enum.GetName(typeof(Accessories), accessory)} failed.");
System.Diagnostics.Debug.WriteLine($"Exception message: {ex.Message}");
Console.WriteLine($"Enabling accessory: {Enum.GetName(typeof(Accessories), accessory)} failed.");
Console.WriteLine($"Exception message: {ex.Message}");
}
}
