58 lines
1.5 KiB
C++
58 lines
1.5 KiB
C++
#include "./motor.h"
|
|
#include "include/config.h"
|
|
#include "./debug.h"
|
|
#include <Arduino.h>
|
|
|
|
|
|
void motorInit()
|
|
{
|
|
pinMode(Config::MotorPinSleep, OUTPUT);
|
|
digitalWrite(Config::MotorPinSleep, LOW);
|
|
|
|
pinMode(Config::MotorPinPWM, OUTPUT);
|
|
digitalWrite(Config::MotorPinPWM, LOW);
|
|
|
|
pinMode(Config::MotorPinDirection, OUTPUT);
|
|
pinMode(Config::MotorPinCurrentSensing, INPUT);
|
|
|
|
// See motorIsOvercurrent
|
|
analogReference(INTERNAL);
|
|
delay(10);
|
|
}
|
|
|
|
|
|
void motorStart(MotorDirection direction)
|
|
{
|
|
//dl("[ MOTOR ] Starting in direction: "); dln((uint8_t)direction);
|
|
|
|
digitalWrite(Config::MotorPinDirection, direction == MotorDirection::Up ? HIGH : LOW);
|
|
digitalWrite(Config::MotorPinSleep, HIGH);
|
|
digitalWrite(Config::MotorPinPWM, HIGH);
|
|
}
|
|
|
|
|
|
void motorStop()
|
|
{
|
|
//dln("[ MOTOR ] Stopping");
|
|
|
|
digitalWrite(Config::MotorPinPWM, LOW);
|
|
digitalWrite(Config::MotorPinSleep, LOW);
|
|
digitalWrite(Config::MotorPinDirection, LOW);
|
|
}
|
|
|
|
|
|
bool motorIsOvercurrent()
|
|
{
|
|
// The Polugu G2 module outputs 20 mV/A plus a 50 mV offset
|
|
// The analog reference has been set to internal, which is 1.1v for the ATMega328P,
|
|
// divided by 1024 units is 1.074mV per unit. This means we can measure up to 52.5A.
|
|
// I sure hope we never reach that value :-)
|
|
auto value = analogRead(Config::MotorPinCurrentSensing);
|
|
float voltage = (float)value * (1100.0 / 1024.0);
|
|
float current = (voltage - 50) / 20;
|
|
|
|
dl("[ MOTOR ] Measured current: value = "); dl(value); dl(", V = "); dl(voltage); dl(", A = "); dln(current);
|
|
|
|
return current >= Config::MotorCurrentLimit;
|
|
}
|