712 lines
17 KiB
C++
712 lines
17 KiB
C++
// ------------------------------------------------------------------------
|
|
// J1939 - Receiving Messages
|
|
// ------------------------------------------------------------------------
|
|
//
|
|
// IMPOPRTANT: Depending on the CAN shield used for this programming sample,
|
|
// please make sure you set the proper CS pin in module can.cpp.
|
|
//
|
|
// This Arduino program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU Lesser General Public
|
|
// License as published by the Free Software Foundation; either
|
|
// version 2.1 of the License, or (at your option) any later version.
|
|
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// Lesser General Public License for more details.
|
|
|
|
#include <stdlib.h>
|
|
// #include <SPI.h>
|
|
|
|
#include "motor.h"
|
|
#include "mcp_can.h"
|
|
#include "can_ext.h"
|
|
// #include "display.h"
|
|
// #include "j1939dtc.h"
|
|
// #include "j1939.h"
|
|
#include "printControl.h"
|
|
|
|
byte nMyAddr = 0;
|
|
#define SA_PREFERRED 128
|
|
#define ADDRESSRANGEBOTTOM 129
|
|
#define ADDRESSRANGETOP 247
|
|
|
|
#define GLOBALADDRESS 255
|
|
#define NULLADDRESS 254 // NAME Fields Default
|
|
|
|
// from Thompson Electrak document 5.2.1
|
|
#define NAME_IDENTITY_NUMBER 0 // 0xFFFFFF
|
|
#define NAME_MANUFACTURER_CODE 547 // 0xFFF
|
|
#define NAME_FUNCTION_INSTANCE 0
|
|
#define NAME_ECU_INSTANCE 0 // 0x01
|
|
#define NAME_FUNCTION 255
|
|
#define NAME_RESERVED 0
|
|
#define NAME_VEHICLE_SYSTEM 0
|
|
#define NAME_VEHICLE_SYSTEM_INSTANCE 0
|
|
#define NAME_INDUSTRY_GROUP 0x00 // Global
|
|
#define NAME_ARBITRARY_ADDRESS_CAPABLE 0x01
|
|
|
|
unsigned char ecuNAME[] =
|
|
{
|
|
(byte)(NAME_IDENTITY_NUMBER & 0xFF),
|
|
(byte)((NAME_IDENTITY_NUMBER >> 8) & 0xFF),
|
|
(byte)(((NAME_MANUFACTURER_CODE << 5) & 0xFF) | (NAME_IDENTITY_NUMBER >> 16)),
|
|
(byte)(NAME_MANUFACTURER_CODE >> 3),
|
|
(byte)((NAME_FUNCTION_INSTANCE << 3) | NAME_ECU_INSTANCE),
|
|
(byte)(NAME_FUNCTION),
|
|
(byte)(NAME_VEHICLE_SYSTEM << 1),
|
|
(byte)((NAME_ARBITRARY_ADDRESS_CAPABLE << 7) | (NAME_INDUSTRY_GROUP << 4) | (NAME_VEHICLE_SYSTEM_INSTANCE))};
|
|
|
|
union u_data
|
|
{
|
|
struct
|
|
{
|
|
byte LB;
|
|
byte HB;
|
|
} v;
|
|
unsigned int w;
|
|
};
|
|
|
|
int motorState;
|
|
|
|
byte blank_response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF};
|
|
void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance,
|
|
byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable);
|
|
|
|
void serviceCAN();
|
|
void motorSync();
|
|
uint16_t motorErrorFlags(byte motor);
|
|
|
|
// J1939 j1939;
|
|
|
|
// Thompson Electrak HD CAN status data
|
|
#define motorPGN 0xEF00
|
|
#define canAddrClaim 0xEE00
|
|
|
|
typedef union
|
|
{
|
|
byte v[8];
|
|
struct
|
|
{
|
|
unsigned position : 14;
|
|
unsigned current : 9;
|
|
unsigned speed : 5;
|
|
unsigned ve : 2;
|
|
unsigned te : 2;
|
|
unsigned motion : 1;
|
|
unsigned overload : 1;
|
|
unsigned backdrive : 1;
|
|
unsigned parameter : 1;
|
|
unsigned saturation : 1;
|
|
unsigned fatalerror : 1;
|
|
unsigned factory : 18;
|
|
unsigned unused : 8;
|
|
} bits;
|
|
} motorStatus_t;
|
|
|
|
// Thompson Electrak HD CAN control data
|
|
typedef union
|
|
{
|
|
byte v[8];
|
|
struct
|
|
{
|
|
unsigned position : 14;
|
|
unsigned current_limit : 9;
|
|
unsigned speed : 5;
|
|
unsigned move : 1;
|
|
unsigned factory : 35;
|
|
} bits;
|
|
} motorControl_t;
|
|
|
|
motorStatus_t mStatus[2];
|
|
motorControl_t mCtl[2];
|
|
bool newData[2];
|
|
|
|
#define addrM 19 // master can address
|
|
#define addrS 20 // slave can address
|
|
|
|
uint8_t M1Speed;
|
|
uint8_t M2Speed;
|
|
|
|
float motorIntegral = 0;
|
|
int16_t maxDelta, minDelta;
|
|
uint8_t maxSpeed;
|
|
uint32_t startTime, runTime;
|
|
uint16_t maxCurrentM, maxCurrentS;
|
|
|
|
bool bDir = 1; // 1 = extend, 0 - retract
|
|
bool bSyncEnable = 1;
|
|
|
|
uint16_t cycleMax;
|
|
uint16_t cycleCount;
|
|
uint16_t cycleDelay;
|
|
|
|
struct
|
|
{
|
|
unsigned master : 1;
|
|
unsigned slave : 1;
|
|
} motor_start;
|
|
|
|
uint16_t startMaster, startSlave;
|
|
uint16_t lastPosition;
|
|
|
|
int32_t slaveCurrentBalance = 0; // v984 DES
|
|
|
|
/******************* end of variables ***********************/
|
|
|
|
// des
|
|
uint16_t getMaxCurrent(uint16_t i)
|
|
{
|
|
if (i == 0)
|
|
return (maxCurrentM);
|
|
return (maxCurrentS);
|
|
}
|
|
|
|
uint16_t getCurrent(uint16_t i)
|
|
{
|
|
if (i == 0)
|
|
return (motorGetCurrent(MASTER));
|
|
return (motorGetCurrent(SLAVE));
|
|
}
|
|
|
|
void setCurrentBalance(int32_t i)
|
|
{
|
|
slaveCurrentBalance = i;
|
|
}
|
|
|
|
// ------------------------------------------------------------------------
|
|
// SYSTEM: Setup routine runs on power-up or reset
|
|
// ------------------------------------------------------------------------
|
|
bool motorInit()
|
|
{
|
|
byte i;
|
|
bool canInit = 0;
|
|
|
|
// j1939.init();
|
|
|
|
/*
|
|
// Set the NAME
|
|
SetNAME(NAME_IDENTITY_NUMBER,
|
|
NAME_MANUFACTURER_CODE,
|
|
NAME_FUNCTION_INSTANCE,
|
|
NAME_ECU_INSTANCE,
|
|
NAME_FUNCTION,
|
|
NAME_VEHICLE_SYSTEM,
|
|
NAME_VEHICLE_SYSTEM_INSTANCE,
|
|
NAME_INDUSTRY_GROUP,
|
|
NAME_ARBITRARY_ADDRESS_CAPABLE);
|
|
|
|
*/
|
|
|
|
// Initialize the CAN controller
|
|
if (canInitialize(CAN_250KBPS) == CAN_OK) // Baud rates defined in mcp_can_dfs.h
|
|
{
|
|
Serial.print("CAN Init OK.\n\r\n\r");
|
|
canInit = 1;
|
|
}
|
|
else
|
|
{
|
|
Serial.print("CAN Init Failed.\n\r");
|
|
return (0);
|
|
delay(1000);
|
|
}
|
|
|
|
j1939Transmit(motorPGN, 6, 255, addrM, (byte *)&mCtl[MASTER], 8);
|
|
j1939Transmit(motorPGN, 6, 255, addrS, (byte *)&mCtl[SLAVE], 8);
|
|
|
|
mCtl[MASTER].bits.position = 0;
|
|
mCtl[MASTER].bits.current_limit = 250;
|
|
|
|
mCtl[SLAVE].bits.position = 0;
|
|
mCtl[SLAVE].bits.current_limit = 250;
|
|
|
|
motorState = MOTOR_IDLE;
|
|
|
|
return (1);
|
|
|
|
} // end setup
|
|
|
|
// ------------------------------------------------------------------------
|
|
// Main Loop - Arduino Entry Point
|
|
// ------------------------------------------------------------------------
|
|
void serviceCan()
|
|
{
|
|
// Declarations
|
|
byte nPriority;
|
|
byte nSrcAddr;
|
|
byte nDestAddr;
|
|
byte nData[8];
|
|
int nDataLen;
|
|
long lPGN;
|
|
long pgnReq;
|
|
byte i;
|
|
|
|
char sString[80];
|
|
|
|
// j1939.tmrControl();
|
|
// Check for received J1939 messages
|
|
if (j1939Receive(&lPGN, &nPriority, &nSrcAddr, &nDestAddr, nData, &nDataLen) == 0)
|
|
{
|
|
if (lPGN != 0)
|
|
{
|
|
if (nDataLen == 0)
|
|
{
|
|
strcpy(printBuff, "\tNo Data");
|
|
printPrint();
|
|
}
|
|
else
|
|
{
|
|
for (int nIndex = 0; nIndex < nDataLen; nIndex++)
|
|
{
|
|
mStatus[nSrcAddr - addrM].v[nIndex] = nData[nIndex];
|
|
// Serial.print( nData[nIndex]); Serial.print(" ");
|
|
}
|
|
// Serial.println();
|
|
}
|
|
|
|
if (nSrcAddr != addrM && nSrcAddr != addrS)
|
|
return;
|
|
switch ((int)lPGN)
|
|
{
|
|
case motorPGN:
|
|
|
|
newData[nSrcAddr - addrM] = 1;
|
|
j1939Transmit(motorPGN, 6, 255, nSrcAddr, (byte *)&mCtl[nSrcAddr - addrM], 8);
|
|
break;
|
|
|
|
case canAddrClaim:
|
|
break;
|
|
|
|
default:
|
|
if (vbCAN)
|
|
{
|
|
sprintf(printBuff, "\r\nUnknown PGN 0x%X %ld", lPGN, lPGN);
|
|
printPrint();
|
|
}
|
|
break;
|
|
} // end 1PGN switch
|
|
}
|
|
} // end if
|
|
} // end loop
|
|
|
|
void motorTask()
|
|
{
|
|
serviceCan();
|
|
|
|
switch (motorState)
|
|
{
|
|
case MOTOR_IDLE:
|
|
if (motor_start.master || motor_start.slave)
|
|
{
|
|
motorState = MOTOR_START;
|
|
}
|
|
currentDir = RUN_IDLE;
|
|
break;
|
|
|
|
case MOTOR_START: //
|
|
|
|
// mStatus[0].bits.position = 0;
|
|
// mStatus[1].bits.position = 0;
|
|
|
|
masterSpeed = parm[P_MOTOR_SPEED];
|
|
motorSetSpeed(MASTER, masterSpeed); // convert duty cycle to 0 - 31
|
|
motorSetSpeed(SLAVE, masterSpeed);
|
|
motorError = MOTOR_ERROR_NONE;
|
|
|
|
motorIntegral = 0.0;
|
|
lastPosition = mStatus[MASTER].bits.position;
|
|
maxDelta = 0;
|
|
minDelta = 0;
|
|
maxSpeed = 0;
|
|
maxCurrentM = 0;
|
|
maxCurrentS = 0;
|
|
startTime = millis();
|
|
Timers[MOTOR_TIMER] = 0;
|
|
printStartOfRun();
|
|
|
|
// start the motors
|
|
mCtl[MASTER].bits.move = motor_start.master;
|
|
mCtl[SLAVE].bits.move = motor_start.slave;
|
|
newData[MASTER] = 0;
|
|
newData[SLAVE] = 0;
|
|
motorState = MOTOR_RAMP;
|
|
break;
|
|
|
|
case MOTOR_RAMP: // wait here until movement is detected
|
|
if (newData[MASTER] == 1 && newData[SLAVE] == 1)
|
|
{
|
|
if ((mStatus[MASTER].bits.motion == 1) && (mStatus[SLAVE].bits.motion == 1))
|
|
{
|
|
motorState = MOTOR_RUN;
|
|
}
|
|
newData[MASTER] = 0;
|
|
newData[SLAVE] = 0;
|
|
}
|
|
if (Timers[MOTOR_TIMER] > parm[30])
|
|
{
|
|
motorError = MOTOR_ERROR_START_TIMEOUT;
|
|
Serial.println("Start Timeout");
|
|
motorState = MOTOR_STOP;
|
|
}
|
|
break;
|
|
|
|
case MOTOR_RUN:
|
|
if (newData[MASTER] == 1 && newData[SLAVE] == 1)
|
|
{
|
|
motorSync();
|
|
if ((mStatus[MASTER].bits.motion == 0) && (mStatus[SLAVE].bits.motion == 0))
|
|
{
|
|
motorState = MOTOR_STOP;
|
|
}
|
|
newData[MASTER] = 0;
|
|
newData[SLAVE] = 0;
|
|
Timers[MOTOR_TIMER] = 0;
|
|
if (mStatus[MASTER].bits.position > lastPosition)
|
|
currentDir = RUN_EXTEND;
|
|
else
|
|
currentDir = RUN_RETRACT;
|
|
}
|
|
if (motorError != MOTOR_ERROR_NONE)
|
|
{
|
|
motorState = MOTOR_STOP;
|
|
}
|
|
|
|
if (Timers[MOTOR_TIMER] > parm[31])
|
|
{
|
|
motorError = MOTOR_ERROR_TIMEOUT;
|
|
mStatus[0].bits.position = 9999;
|
|
mStatus[1].bits.position = 9999;
|
|
Serial.println("Run Timeout");
|
|
}
|
|
break;
|
|
|
|
case MOTOR_STOP:
|
|
runTime = (millis() - startTime);
|
|
printEndOfRun();
|
|
mCtl[MASTER].bits.move = 0;
|
|
mCtl[SLAVE].bits.move = 0;
|
|
motor_start.master = 0;
|
|
motor_start.slave = 0;
|
|
motorState = MOTOR_IDLE;
|
|
break;
|
|
|
|
default:
|
|
Serial.print("Bad State ");
|
|
Serial.println(motorState);
|
|
sprintf(printBuff, "Unknown State %d", motorState);
|
|
printPrint();
|
|
cycleCount = 0;
|
|
motorState = MOTOR_STOP;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// *************** Start/Stop ***********************
|
|
void motorMove(uint16_t position)
|
|
{
|
|
mCtl[MASTER].bits.position = position;
|
|
mCtl[SLAVE].bits.position = position;
|
|
motor_start.master = 1;
|
|
motor_start.slave = 1;
|
|
if (position > motorGetPosition(MASTER))
|
|
motorDir = EXTEND;
|
|
else
|
|
motorDir = RETRACT;
|
|
}
|
|
|
|
void motorStart(uint8_t motor)
|
|
{
|
|
if (motorState == MOTOR_IDLE)
|
|
{
|
|
motor_start.master = (motor & 1);
|
|
motor_start.slave = (motor & 2) >> 1;
|
|
motorState = MOTOR_START;
|
|
}
|
|
}
|
|
|
|
void motorStop()
|
|
{
|
|
if (motorState != MOTOR_IDLE)
|
|
{
|
|
motor_start.master = 0;
|
|
motor_start.slave = 0;
|
|
motorState = MOTOR_STOP;
|
|
}
|
|
}
|
|
|
|
// ************* Speed **************************
|
|
void motorSetSpeed(uint8_t motor, uint8_t duty)
|
|
{
|
|
uint16_t temp;
|
|
char strTemp[30];
|
|
temp = duty;
|
|
temp *= 32;
|
|
temp /= 100;
|
|
if (temp > 31)
|
|
temp = 31;
|
|
mCtl[motor].bits.speed = temp;
|
|
}
|
|
|
|
uint8_t motorGetSetSpeed(uint8_t motor)
|
|
{
|
|
uint16_t duty;
|
|
duty = mCtl[motor].bits.speed;
|
|
duty = (duty * 100) / 32;
|
|
return ((uint8_t)duty);
|
|
}
|
|
uint8_t motorGetActualSpeed(uint8_t motor)
|
|
{
|
|
uint16_t duty;
|
|
duty = mStatus[motor].bits.speed;
|
|
duty = (duty * 100) / 32;
|
|
return ((uint8_t)duty);
|
|
}
|
|
|
|
//************ Position ******************************
|
|
void motorSetPosition(uint8_t motor, uint16_t position)
|
|
{
|
|
mCtl[motor].bits.position = (position & 0x3FFF);
|
|
}
|
|
uint16_t motorGetPosition(uint8_t motor)
|
|
{
|
|
return (mStatus[motor].bits.position);
|
|
}
|
|
//*********** Current ********************************
|
|
void motorSetCurrentLimit(uint8_t motor, uint16_t limit)
|
|
{
|
|
mCtl[motor].bits.current_limit = (limit & 0x1FF);
|
|
}
|
|
uint16_t motorGetCurrent(uint8_t motor)
|
|
{
|
|
return (mStatus[motor].bits.current);
|
|
}
|
|
//************* Flags *********************************
|
|
uint8_t motorGetFlags(uint8_t motor, uint8_t flag)
|
|
{
|
|
uint8_t data = 0;
|
|
switch (flag)
|
|
{
|
|
case MOTOR_FLAG_VOLTAGE_ERROR:
|
|
data = mStatus[motor].bits.ve;
|
|
break;
|
|
case MOTOR_FLAG_TEMP_ERROR:
|
|
data = mStatus[motor].bits.te;
|
|
break;
|
|
case MOTOR_FLAG_MOTION:
|
|
data = mStatus[motor].bits.motion;
|
|
break;
|
|
case MOTOR_FLAG_OVERLOAD:
|
|
data = mStatus[motor].bits.overload;
|
|
break;
|
|
case MOTOR_FLAG_BACKDRIVE:
|
|
data = mStatus[motor].bits.backdrive;
|
|
break;
|
|
case MOTOR_FLAG_PARAMETER:
|
|
data = mStatus[motor].bits.parameter;
|
|
break;
|
|
case MOTOR_FLAG_SATURATION:
|
|
data = mStatus[motor].bits.saturation;
|
|
break;
|
|
case MOTOR_FLAG_FATAL_ERROR:
|
|
data = mStatus[motor].bits.fatalerror;
|
|
break;
|
|
}
|
|
return (data);
|
|
}
|
|
|
|
/*************************************************************************
|
|
Synchronize motors
|
|
M1 will always run at set speed, M2 will be adjusted to match
|
|
M1 encoder counter
|
|
|
|
M2Speed = (delta * P/100) + (I/100 + IG) + M1Speed(masterSpeed)
|
|
|
|
**************************************************************************/
|
|
void motorSync(void)
|
|
{
|
|
|
|
float delta;
|
|
float newSpeed;
|
|
static byte syncCounter = 0;
|
|
uint16_t slavePosition; // V504
|
|
|
|
char sString[30];
|
|
|
|
if (++syncCounter < parm[P_MOTOR_ADJ_INTERVAL])
|
|
return;
|
|
|
|
syncCounter = 0;
|
|
// slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET]; //***** V504
|
|
|
|
slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET] + slaveCurrentBalance; //***** V983 DES
|
|
|
|
if (motorDir == EXTEND)
|
|
delta = mStatus[MASTER].bits.position - slavePosition; // V504
|
|
else
|
|
delta = ((3000 - mStatus[MASTER].bits.position) - (3000 - slavePosition)); // V504
|
|
|
|
if (delta == 0)
|
|
return;
|
|
|
|
// if the motors get too far apart, shutdown
|
|
if (abs(delta) > parm[P_MAX_DELTA])
|
|
{
|
|
// shut down
|
|
motorError = MOTOR_ERROR_MAX_DELTA;
|
|
/*
|
|
Serial.printf("DELTA Error %d %d\n",mStatus[MASTER].bits.position,slavePosition);
|
|
Serial.printf("Parm 44 = %d, Delta = %d\n",parm[44],delta);
|
|
Serial.printf("DELTA Error %d %d\n",3000 - mStatus[MASTER].bits.position,3000 - slavePosition);
|
|
while(1);
|
|
*/
|
|
return;
|
|
}
|
|
M1Speed = masterSpeed;
|
|
|
|
if (delta > 0)
|
|
if (delta > maxDelta)
|
|
maxDelta = delta;
|
|
|
|
if (delta < 0)
|
|
if (delta < minDelta)
|
|
minDelta = (int16_t)delta;
|
|
|
|
if (vbSync)
|
|
{
|
|
dtostrf(delta, 4, 0, sString);
|
|
sprintf(sString, "%s,%u,%u,", sString, motorGetPosition(MASTER), motorGetPosition(SLAVE));
|
|
strcpy(printBuff, sString);
|
|
}
|
|
|
|
if (delta < 0) // slave is ahead of master
|
|
motorIntegral -= (float)parm[P_MOTOR_ADJ_I] / 100.0;
|
|
else
|
|
motorIntegral += (float)parm[P_MOTOR_ADJ_I] / 100.0;
|
|
if (vbSync)
|
|
{
|
|
dtostrf(motorIntegral, 6, 2, sString);
|
|
sprintf(sString, "%s,", sString);
|
|
strcat(printBuff, sString);
|
|
}
|
|
newSpeed = (delta * (float)parm[P_MOTOR_ADJ_P]) / 100.0;
|
|
|
|
if (vbSync)
|
|
{
|
|
dtostrf(newSpeed, 6, 2, sString);
|
|
sprintf(sString, "%s,", sString);
|
|
strcat(printBuff, sString);
|
|
}
|
|
|
|
newSpeed += motorIntegral;
|
|
|
|
if (vbSync)
|
|
{
|
|
dtostrf(newSpeed, 6, 2, sString);
|
|
sprintf(sString, "%s,", sString);
|
|
strcat(printBuff, sString);
|
|
// Serial.print(newSpeed,3);
|
|
// Serial.print(",");
|
|
}
|
|
|
|
newSpeed += M1Speed;
|
|
|
|
if (vbSync)
|
|
{
|
|
dtostrf(newSpeed, 6, 2, sString);
|
|
sprintf(sString, "%s,", sString);
|
|
strcat(printBuff, sString);
|
|
}
|
|
|
|
if (newSpeed >= 20 && newSpeed <= 100)
|
|
{
|
|
M2Speed = (byte)newSpeed;
|
|
}
|
|
else if (newSpeed > 100)
|
|
{
|
|
M2Speed = 100;
|
|
}
|
|
else
|
|
{
|
|
M2Speed = 20;
|
|
}
|
|
if (M2Speed > maxSpeed)
|
|
maxSpeed = M2Speed;
|
|
|
|
if (bSyncEnable)
|
|
motorSetSpeed(SLAVE, M2Speed);
|
|
|
|
if (vbSync)
|
|
{
|
|
sprintf(sString, "%u,%04u,%04u,%04u,%04u",
|
|
M2Speed, motorGetSetSpeed(MASTER), motorGetSetSpeed(SLAVE),
|
|
motorGetCurrent(MASTER), motorGetCurrent(SLAVE));
|
|
strcat(printBuff, sString);
|
|
printPrint();
|
|
}
|
|
|
|
if (motorGetCurrent(MASTER) > maxCurrentM)
|
|
maxCurrentM = motorGetCurrent(MASTER);
|
|
if (motorGetCurrent(SLAVE) > maxCurrentS)
|
|
maxCurrentS = motorGetCurrent(SLAVE);
|
|
}
|
|
|
|
void motorJog(uint8_t motor, uint8_t state, uint16_t position)
|
|
{
|
|
mCtl[motor].bits.position = position;
|
|
mCtl[motor].bits.move = ~state;
|
|
|
|
if (state == 1) // button released
|
|
{
|
|
if (vb)
|
|
printPosition();
|
|
}
|
|
}
|
|
|
|
uint16_t motorErrorFlags(byte motor)
|
|
{
|
|
uint16_t flags;
|
|
|
|
flags = mStatus[motor].v[4] >> 4;
|
|
flags += (mStatus[motor].v[5] << 4);
|
|
return flags;
|
|
}
|
|
|
|
uint16_t getMotorError(void)
|
|
{
|
|
return (motorError);
|
|
}
|
|
|
|
void clearMotorError(void)
|
|
{
|
|
motorError = 0;
|
|
mStatus[0].bits.position = 0;
|
|
mStatus[1].bits.position = 0;
|
|
}
|
|
|
|
// SetNAME is not needed
|
|
|
|
// void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance,
|
|
// byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable)
|
|
// {
|
|
// byte i;
|
|
// Serial.print("0X");
|
|
// for(i = 0; i < 8; i++)
|
|
// Serial.print(ecuNAME[7-i],HEX);
|
|
|
|
// Serial.println("\r\n");
|
|
|
|
// ecuNAME[0] = (byte)(v81 & 0xFF);
|
|
// ecuNAME[1] = (byte)((v81 >> 8) & 0xFF);
|
|
// ecuNAME[2] = (byte)(((nManufacturerCode << 5) & 0xFF) | (v81 >> 16));
|
|
// ecuNAME[3] = (byte)(nManufacturerCode >> 3);
|
|
// ecuNAME[4] = (byte)((nFunctionInstance << 3) | nECUInstance);
|
|
// ecuNAME[5] = (byte)(nFunction);
|
|
// ecuNAME[6] = (byte)(nVehicleSystem << 1);
|
|
// ecuNAME[7] = (byte)((nArbitraryAddressCapable << 7) | (nIndustryGroup << 4) | (nVehicleSystemInstance));
|
|
// Serial.print("0X");
|
|
// for(i = 0; i < 8; i++)
|
|
// Serial.print(ecuNAME[7-i],HEX);
|
|
|
|
// Serial.println("\r\n");
|
|
// }
|