345 lines
10 KiB
C++
345 lines
10 KiB
C++
#include <Arduino.h>
|
|
#include "motors.h"
|
|
#include "vehicleConfig.h"
|
|
#include "rc_board.h"
|
|
#include "radio.h"
|
|
#include "lights.h"
|
|
#include "steeringCurves.h"
|
|
#include "balancing.h"
|
|
#include "MPU6050.h"
|
|
|
|
#include "MC34933.h"
|
|
#include "PWMFrequency.h"
|
|
|
|
bool isDriving; // is the vehicle driving?
|
|
// Motor objects
|
|
MC34933 Motor1;
|
|
MC34933 Motor2;
|
|
|
|
|
|
// This array is intended for the "vhctype_semi caterpillar" mode. The inner wheel can max. slow down to 60% of the
|
|
// outer wheels RPM
|
|
float curvevhctype_semi[][2] = { // see excel sheet!
|
|
{0, 60} // {input value, output value}
|
|
, {25, 70}
|
|
, {50, 80}
|
|
, {75, 90}
|
|
, {100, 100}
|
|
};
|
|
|
|
// This array is intended for the "Caterpillar" mode. The inner wheel can spin backwars up to 100% of the
|
|
// outer wheels RPM. That allows for turning the vehicle "on place"
|
|
float curveFull[][2] = {
|
|
{0, -100} // {input value, output value}
|
|
, {7, 0}
|
|
, {20, 45}
|
|
, {60, 85}
|
|
, {100, 100}
|
|
};
|
|
|
|
// This array is intended for the "Differential Thrust" mode. The inner motor can max. slow down to 20% of the
|
|
// outer motors RPM
|
|
float curveThrust[][2] = { // see excel sheet!
|
|
{0, 20} // {input value, output value}
|
|
, {25, 40}
|
|
, {50, 60}
|
|
, {75, 80}
|
|
, {100, 100}
|
|
};
|
|
|
|
//
|
|
// =======================================================================================================
|
|
// MOTOR DRIVER SETUP
|
|
// =======================================================================================================
|
|
//
|
|
|
|
void setupMotors()
|
|
{
|
|
vehicleConfig* cfg = getConfigptr();
|
|
// MC34933 H-Bridge pins
|
|
const byte motor1_in1 = PIN_MOTOR_M1A;
|
|
const byte motor1_in2 = PIN_MOTOR_M1B;
|
|
|
|
const byte motor2_in1 = PIN_MOTOR_M2A;
|
|
const byte motor2_in2 = PIN_MOTOR_M2B;
|
|
|
|
|
|
// SYNTAX: IN1, IN2, PWM, min. input value, max. input value, neutral position width
|
|
// invert rotation direction true or false
|
|
Motor2.begin(motor1_in1, motor1_in2, 0, 100, 4, false); // Drive motor
|
|
Motor1.begin(motor2_in1, motor2_in2, 0, 100, 4, false); // Steering motor (Drive in "HP" version)
|
|
|
|
// Motor PWM frequency prescalers (Requires the PWMFrequency.h library)
|
|
// Differential steering vehicles: locked to 984Hz, to make sure, that both motors use 984Hz.
|
|
if (cfg->getvehicleType() == vhctype_semi || cfg->getvehicleType() == vhctype_unknown || cfg->getvehicleType() == simpledualmotorplane)
|
|
{
|
|
cfg->setpwmPrescaler2(32);
|
|
}
|
|
// ----------- IMPORTANT!! --------------
|
|
// Motor 1 always runs @ 984Hz PWM frequency and can't be changed, because timers 0 an 1 are in use for other things!
|
|
// Motor 2 (pin 3) can be changed to the following PWM frequencies: 32 = 984Hz, 8 = 3936Hz, 1 = 31488Hz
|
|
setPWMPrescaler(3, cfg->getpwmPrescaler2()); // pin 3 is hardcoded, because we can't change all others anyway
|
|
}
|
|
|
|
|
|
MC34933* getMotorptr(motor_t motor)
|
|
{
|
|
switch(motor)
|
|
{
|
|
case MOTOR1:
|
|
{
|
|
return &Motor1;
|
|
}
|
|
break;
|
|
|
|
case MOTOR2:
|
|
{
|
|
return &Motor2;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
{
|
|
return NULL;
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
// =======================================================================================================
|
|
// DRIVE MOTORS CAR (for cars, one motor for driving, one for steering)
|
|
// =======================================================================================================
|
|
//
|
|
|
|
void motordrive(motor_t motor, int controlValue, int minPWM, int maxPWM, int rampTime, bool neutralBrake)
|
|
{
|
|
getMotorptr(motor)->drive( controlValue, minPWM, maxPWM, rampTime, neutralBrake);
|
|
}
|
|
|
|
bool motorBrakeActive(motor_t motor)
|
|
{
|
|
return getMotorptr(motor)->brakeActive();
|
|
}
|
|
|
|
void drive(_mpu* mpu)
|
|
{
|
|
vehicleConfig* cfg = getConfigptr();
|
|
|
|
switch(cfg->getvehicleType())
|
|
{
|
|
case car:
|
|
{
|
|
driveMotorsCar();
|
|
}
|
|
break;
|
|
|
|
case carwithMRSC:
|
|
{
|
|
mrsc(mpu); // Car with MSRC stabilty control
|
|
}
|
|
break;
|
|
|
|
case forklift:
|
|
{
|
|
driveMotorsForklift(); // Forklift
|
|
}
|
|
break;
|
|
|
|
case balancingthing:
|
|
{
|
|
balancing(mpu); // Self balancing robot
|
|
}
|
|
break;
|
|
|
|
default:
|
|
{
|
|
driveMotorsSteering(); // Caterpillar and half caterpillar vecicles
|
|
}
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
void driveMotorsCar() {
|
|
|
|
vehicleConfig* cfg = getConfigptr();
|
|
RcData* data = getDataptr();
|
|
//ackPayload* payload = getPayloadptr();
|
|
|
|
int maxPWM;
|
|
byte maxAcceleration;
|
|
|
|
// Speed limitation (max. is 255)
|
|
if (data->mode1) {
|
|
maxPWM = cfg->getmaxPWMlimited(); // Limited
|
|
} else {
|
|
maxPWM = cfg->getmaxPWMfull(); // Full
|
|
}
|
|
|
|
if (!data->batteryOk && cfg->getliPo()) data->axis3 = 50; // Stop the vehicle, if the battery is empty!
|
|
|
|
// Acceleration & deceleration limitation (ms per 1 step input signal change)
|
|
if (data->mode2) {
|
|
maxAcceleration = cfg->getmaxAccelerationLimited(); // Limited
|
|
} else {
|
|
maxAcceleration = cfg->getmaxAccelerationFull(); // Full
|
|
}
|
|
|
|
// ***************** Note! The ramptime is intended to protect the gearbox! *******************
|
|
// SYNTAX: Input value, max PWM, ramptime in ms per 1 PWM increment
|
|
// false = brake in neutral position inactive
|
|
|
|
if (!cfg->getHP()) { // Two channel version: ----
|
|
if (Motor1.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral)
|
|
UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving!
|
|
}
|
|
if (cfg->getvehicleType() != carwithMRSC) { // If not car with MSRC stabilty control
|
|
Motor2.drive(data->axis1, 0, cfg->getsteeringTorque(), 0, false); // The steering motor (if the original steering motor is reused instead of a servo)
|
|
}
|
|
}
|
|
else { // High Power "HP" version. Motor 2 is the driving motor, no motor 1: ----
|
|
if (Motor2.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral)
|
|
UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving!
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
// =======================================================================================================
|
|
// DRIVE MOTORS FORKLIFT (for forklifts, one motor for driving, one for lifting)
|
|
// =======================================================================================================
|
|
//
|
|
|
|
void driveMotorsForklift() {
|
|
|
|
vehicleConfig* cfg = getConfigptr();
|
|
RcData* data = getDataptr();
|
|
//ackPayload* payload = getPayloadptr();
|
|
|
|
int maxPWM;
|
|
byte maxAcceleration;
|
|
|
|
// Speed limitation (max. is 255)
|
|
if (data->mode1) {
|
|
maxPWM = cfg->getmaxPWMlimited(); // Limited
|
|
} else {
|
|
maxPWM = cfg->getmaxPWMfull(); // Full
|
|
}
|
|
|
|
if (!data->batteryOk && cfg->getliPo()) data->axis3 = 50; // Stop the vehicle, if the battery is empty!
|
|
|
|
// Acceleration & deceleration limitation (ms per 1 step input signal change)
|
|
if (data->mode2) {
|
|
maxAcceleration = cfg->getmaxAccelerationLimited(); // Limited
|
|
} else {
|
|
maxAcceleration = cfg->getmaxAccelerationFull(); // Full
|
|
}
|
|
|
|
// ***************** Note! The ramptime is intended to protect the gearbox! *******************
|
|
// SYNTAX: Input value, max PWM, ramptime in ms per 1 PWM increment
|
|
// false = brake in neutral position inactive
|
|
|
|
|
|
if (Motor1.drive(data->axis3, cfg->getminPWM(), maxPWM, maxAcceleration, true) ) { // The drive motor (function returns true, if not in neutral)
|
|
UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving!
|
|
}
|
|
Motor2.drive(data->axis2, 0, cfg->getsteeringTorque(), 0, false); // The fork lifting motor (the steering is driven by servo 1)
|
|
}
|
|
|
|
//
|
|
// =======================================================================================================
|
|
// "STEERING" MOTOR DRIVING FUNCTION (throttle & steering overlay for caterpillar vehicles)
|
|
// =======================================================================================================
|
|
//
|
|
|
|
bool getIsDriving()
|
|
{
|
|
return isDriving;
|
|
}
|
|
|
|
void setIsDriving(bool driveing)
|
|
{
|
|
isDriving = driveing;
|
|
}
|
|
|
|
|
|
void driveMotorsSteering() {
|
|
|
|
int pwm[2];
|
|
|
|
vehicleConfig* cfg = getConfigptr();
|
|
RcData* data = getDataptr();
|
|
//ackPayload* payload = getPayloadptr();
|
|
|
|
// The steering overlay
|
|
int steeringFactorLeft=0;
|
|
int steeringFactorRight=0;
|
|
int steeringFactorLeft2=0;
|
|
int steeringFactorRight2 =0;
|
|
|
|
// The input signal range
|
|
const int servoMin = 0;
|
|
const int servoMax = 100;
|
|
const int servoNeutralMin = 48;
|
|
const int servoNeutralMax = 52;
|
|
|
|
// Compute steering overlay:
|
|
// The steering signal is channel 1 = data.axis1
|
|
// 100% = wheel spins with 100% of the requested speed forward
|
|
// -100% = wheel spins with 100% of the requested speed backward
|
|
if (data->axis1 <= servoNeutralMin) {
|
|
steeringFactorLeft = map(data->axis1, servoMin, servoNeutralMin, 0, 100);
|
|
steeringFactorLeft = constrain(steeringFactorLeft, 0, 100);
|
|
}
|
|
else {
|
|
steeringFactorLeft = 100;
|
|
}
|
|
|
|
if (data->axis1 >= servoNeutralMax) {
|
|
steeringFactorRight = map(data->axis1, servoMax, servoNeutralMax, 0, 100);
|
|
steeringFactorRight = constrain(steeringFactorRight, 0, 100);
|
|
}
|
|
else {
|
|
steeringFactorRight = 100;
|
|
}
|
|
|
|
// Nonlinear steering overlay correction
|
|
if (cfg->getvehicleType() == 6) {
|
|
steeringFactorLeft2 = reMap(curveThrust, steeringFactorLeft); // Differential thrust mode
|
|
steeringFactorRight2 = reMap(curveThrust, steeringFactorRight);
|
|
data->axis3 = constrain(data->axis3, 50, 100); // reverse locked!
|
|
}
|
|
if (cfg->getvehicleType() == 2) {
|
|
steeringFactorLeft2 = reMap(curveFull, steeringFactorLeft); // Caterpillar mode
|
|
steeringFactorRight2 = reMap(curveFull, steeringFactorRight);
|
|
}
|
|
if (cfg->getvehicleType() == 1) {
|
|
steeringFactorLeft2 = reMap(curvevhctype_semi, steeringFactorLeft); // vhctype_semi caterpillar mode
|
|
steeringFactorRight2 = reMap(curvevhctype_semi, steeringFactorRight);
|
|
}
|
|
|
|
// Drive caterpillar motors
|
|
// The throttle signal (for both caterpillars) is channel 3 = data.axis3
|
|
// -100 to 100%
|
|
pwm[0] = map(data->axis3, servoMin, servoMax, 100, -100) * steeringFactorRight2 / 100;
|
|
pwm[1] = map(data->axis3, servoMin, servoMax, 100, -100) * steeringFactorLeft2 / 100;
|
|
|
|
pwm[0] = map(pwm[0], 100, -100, 100, 0); // convert -100 to 100% to 0-100 for motor control
|
|
pwm[1] = map(pwm[1], 100, -100, 100, 0);
|
|
|
|
if (!data->batteryOk && cfg->getliPo()) { // Both motors off, if LiPo battery is empty!
|
|
pwm[0] = 0;
|
|
pwm[1] = 0;
|
|
}
|
|
|
|
Motor1.drive(pwm[0], 0, 255, 0, false); // left caterpillar, 0ms ramp!
|
|
Motor2.drive(pwm[1], 0, 255, 0, false); // right caterpillar
|
|
|
|
if (pwm[0] < 40 || pwm[0] > 60 || pwm[1] < 40 || pwm[1] > 60) {
|
|
UpdatemillisLightOff(); // Reset the headlight delay timer, if the vehicle is driving!
|
|
}
|
|
} |