New power supply arrived and I have tested to spin the MGR using 80V and YES, it spins!!
At the moment it only works in reverse gear and wheels in the air so not producing a lot of torque.
Will try to figure out why it woun't work in forward. It only consumes power and my thinking is that the inverter tries to do regen or something so will have a play with signing of torque request and other things.
The SW used.
Code: Select all
// Change log
/*
2_1
* Changes to Inverter.Status struct
* Changed how dcBusVoltage is calculated in controlInverter()
* Changed to Serial.write for gear print out in printData()
2
* Changes in setup() to get USART working correctly
1
* Initial version based on GS450h_v3 and GS450h.cpp, by T.Darby and D.Maguire
*/
// Include libraries
#include "due_can.h"
// Constant declaration
const uint16_t maxSpeed = 10000;
const uint8_t htm_data_init[7][100]=
{
{0,14,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,4,0,25,0,0,0,0,0,0,0,0,0,0,136,0,0,0,160,0,0,0,0,0,0,0,95,1},
{0,14,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,4,0,25,0,0,0,0,0,0,0,0,0,0,136,0,0,0,160,0,0,0,0,0,0,0,95,1},
{0,30,0,0,0,0,0,18,0,154,250,0,0,0,0,97,4,0,0,0,0,0,173,255,82,0,0,0,0,0,0,0,16,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,4,75,25,60,246,52,8,0,0,0,0,0,0,138,0,0,0,168,0,0,0,1,0,0,0,72,7},
{0,30,0,0,0,0,0,18,0,154,250,0,0,0,0,97,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,16,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,4,75,25,60,246,52,8,0,0,0,0,0,0,138,0,0,0,168,0,0,0,2,0,0,0,75,5},
{0,30,0,0,0,0,0,18,0,154,250,0,0,0,0,97,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,16,0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,0,4,75,25,60,246,52,8,0,0,0,0,0,0,138,0,0,0,168,0,0,0,2,0,0,0,75,5},
{0,30,0,0,0,0,0,18,0,154,250,0,0,255,0,97,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,16,0,0,0,0,0,255,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,255,4,73,25,60,246,52,8,0,0,255,0,0,0,138,0,0,0,168,0,0,0,3,0,0,0,70,9},
{0,30,0,2,0,0,0,18,0,154,250,0,0,16,0,97,0,0,0,0,0,0,200,249,56,6,165,0,136,0,63,0,16,0,0,0,63,0,16,0,3,128,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,0,0,0,0,16,0,75,12,45,248,21,6,0,0,16,0,0,0,202,0,211,0,16,0,0,0,134,16,0,0,130,10}
};
// Variable declaration
uint8_t htm_data[100];
uint8_t mth_data[120];
uint16_t inCounter = 0;
uint16_t outCounter = 0;
struct InverterRequest {
int16_t mg1Torque = 0;
int16_t mg2Torque = 0;
uint8_t gear = 'N';
} inverterRequest;
struct InverterStatus {
uint16_t dcBusVoltage = 0;
int16_t tempInvWater = 0;
int16_t tempInvInductor = 0;
int16_t mg1Speed = 0;
int16_t mg2Speed = 0;
} inverterStatus;
// I/O-PINS
const uint8_t reqPIN = 22; // D22
const uint8_t invPwrPIN = 54; // D54, inverter 12V supply
const uint8_t igctPIN = 57; // D57, dc/dc 12V supply
const uint8_t vloPIN = 60; // D60, voltage cont for dc/dc
const uint8_t mgrTempPIN = 61; // A7, MGR stator temperature
const uint8_t drivePIN = 7; // D7
const uint8_t reversePIN = 6; // D6
const uint8_t accPIN = 63; // A9
/********
* SETUP *
********/
void setup()
{
pinMode(reqPIN, OUTPUT);
digitalWrite(reqPIN, HIGH);
pinMode(invPwrPIN, OUTPUT);
digitalWrite(invPwrPIN, LOW);
pinMode(igctPIN, OUTPUT);
digitalWrite(igctPIN, LOW);
pinMode(vloPIN, OUTPUT);
digitalWrite(vloPIN, LOW);
pinMode(drivePIN, INPUT_PULLUP);
pinMode(reversePIN, INPUT_PULLUP);
Serial1.begin(250000);
PIOA->PIO_ABSR |= 1<<17;
PIOA->PIO_PDR |= 1<<17;
USART0->US_MR |= 1<<4 | 1<<8 | 1<<18;
Can0.begin(CAN_BPS_250K); //CAN bus at 250kbit/s
Serial.begin(115200);
Serial.print("Setup done!");
}
/*******
* LOOP *
*******/
void loop()
{
controlInverter();
printData();
}
/***************************************************
* Prepare data and send it to the Inverter (Motor) *
***************************************************/
void controlInverter()
{
static uint32_t lastMillisData = millis();
static uint32_t lastMillisInput = millis();
if (millis() - lastMillisData >= 1) // 1ms
{
static uint8_t dataState = 0;
static uint8_t setupState = 0;
lastMillisData = millis(); // Reset timer
if(0 == dataState)
{
uint8_t mth_byte = 0;
digitalWrite(reqPIN, LOW);
for(int i=0; i<120; i++) // Clear array
{
mth_data[i]=0;
}
while(Serial1.available()) // Receive data
{
mth_data[mth_byte] = Serial1.read();
mth_byte++;
}
if(mth_byte >= 120) // Increase counter if all data received
{
inCounter++;
}
dataState++;
}
else if(1 == dataState)
{
digitalWrite(reqPIN, HIGH);
if(setupState > 5)
{
for(int i=0; i<100; i++)
{
Serial1.write(htm_data[i]);
}
outCounter++;
}
else
{
for(int i=0; i<100; i++)
{
Serial1.write(htm_data_init[setupState][i]);
}
setupState++;
outCounter++;
}
dataState++;
}
else if(2 == dataState)
{
dataState++;
}
else if(3 == dataState)
{
if(verifyMTHChecksum())
{
//inverterStatus.dcBusVoltage = ((mth_data[100] | mth_data[101] << 8) - 5) / 2;
//inverterStatus.dcBusVoltage = ((mth_data[100] | mth_data[101] << 8) - 0) * 0.615;
inverterStatus.dcBusVoltage = ((mth_data[100] | mth_data[101] << 8) - 0) * 30 / 49;
inverterStatus.tempInvWater = mth_data[42] | mth_data[43] << 8;
inverterStatus.tempInvInductor = mth_data[86] | mth_data[87] << 8;
inverterStatus.mg1Speed = mth_data[6] | mth_data[7] << 8;
inverterStatus.mg2Speed = mth_data[38] | mth_data[39] << 8;
}
else
{
// Error
}
dataState++;
}
else if(4 == dataState)
{
static boolean frameCount = false;
// MG1
htm_data[76] = (inverterRequest.mg1Torque * 4) & 0xFF;
htm_data[75] = ((inverterRequest.mg1Torque * 4) >> 8) & 0xFF;
htm_data[5] = (inverterRequest.mg1Torque) & 0xFF; // Negative is forward
htm_data[6] = ((inverterRequest.mg1Torque) >> 8);
htm_data[11] = htm_data[5];
htm_data[12] = htm_data[6];
// MG2
htm_data[30] = (inverterRequest.mg2Torque) & 0xFF; // Positive is forward
htm_data[31] = ((inverterRequest.mg2Torque) >> 8) & 0xFF;
if(inverterRequest.mg2Torque > 0)
{
// Forward direction these bytes should match
htm_data[26] = htm_data[30];
htm_data[27] = htm_data[31];
htm_data[28] = (inverterRequest.mg2Torque / 2) & 0xFF; // Positive is forward
htm_data[29] = ((inverterRequest.mg2Torque / 2) >> 8) & 0xFF;
}
if(inverterRequest.mg2Torque < 0)
{
// Reverse direction these bytes should match
htm_data[28] = htm_data[30];
htm_data[29] = htm_data[31];
htm_data[26] = (inverterRequest.mg2Torque / 2) & 0xFF; // Negative is reverse
htm_data[27] = ((inverterRequest.mg2Torque / 2) >> 8) & 0xFF;
}
// Other settings
htm_data[85] = (-5000) & 0xFF; // Charge ability of battery
htm_data[86] = ((-5000) >> 8);
htm_data[87] = (-10000) & 0xFF; // Discharge ability of battery
htm_data[88] = ((-10000) >> 8);
/*// Checksum
if(++frame_count & 0x01)
{
htm_data[94]++;
}*/
// Checksum
if(frameCount)
{
htm_data[94]++;
}
frameCount = !frameCount;
calcHTMChecksum();
dataState = 0;
}
else
{
// Error
}
}
if (millis() - lastMillisInput >= 10) // 10ms, read input and convert to torque command
{
lastMillisInput = millis(); // Reset timer
if(!digitalRead(drivePIN) && digitalRead(reversePIN))
{
inverterRequest.gear = 'D';
}
if(digitalRead(drivePIN) && !digitalRead(reversePIN))
{
inverterRequest.gear = 'R';
}
if(digitalRead(drivePIN) && digitalRead(reversePIN))
{
inverterRequest.gear = 'N';
}
uint16_t accPedal = analogRead(accPIN);
accPedal = max(100, accPedal);
accPedal = min(900, accPedal);
if('N' == inverterRequest.gear)
{
inverterRequest.mg2Torque = 0;
}
else if('D' == inverterRequest.gear)
{
inverterRequest.mg2Torque = map(accPedal, 100, 900, 0, 1000);
}
else if('R' == inverterRequest.gear)
{
inverterRequest.mg2Torque = map(accPedal, 100, 900, 0, -1000);
}
if((inverterStatus.mg2Speed > maxSpeed) || (inverterStatus.mg2Speed < -maxSpeed))
{
inverterRequest.mg2Torque = 0;
}
}
}
/*********************************
* Check if checksum matches data *
*********************************/
boolean verifyMTHChecksum()
{
uint16_t mth_checksum = 0;
for(int i=0; i<118; i++)
{
mth_checksum += mth_data[i];
}
/*if((mth_data[118] | (mth_data[119] << 8)) == mth_checksum)
{
return true;
}
else
{
return false;
}*/
boolean checksumStatus = ((mth_data[118] | (mth_data[119] << 8)) == mth_checksum);
return checksumStatus;
}
/****************************************
* Calculates checksum for outgoing data *
****************************************/
void calcHTMChecksum()
{
uint16_t htm_checksum = 0;
for(int i=0; i<(98); i++)
{
htm_checksum += htm_data[i];
}
htm_data[98] = htm_checksum & 0xFF;
htm_data[99] = htm_checksum >> 8;
}
/***************************
* Sends data to USB-Serial *
***************************/
void printData()
{
static uint32_t lastMillisPrint = millis();
if (millis() - lastMillisPrint >= 1000) // 1000ms
{
lastMillisPrint = millis(); // Reset timer
// Inverter request
Serial.print("Request, MG1_T: ");
Serial.print(inverterRequest.mg1Torque);
Serial.print(" MG2_T: ");
Serial.print(inverterRequest.mg2Torque);
Serial.print(" Gear: ");
Serial.write(inverterRequest.gear);
Serial.println();
// Inverter data
Serial.print("Data, DC_bus: ");
Serial.print(inverterStatus.dcBusVoltage);
Serial.print(" Temp W: ");
Serial.print(inverterStatus.tempInvWater);
Serial.print(" Temp I: ");
Serial.print(inverterStatus.tempInvInductor);
Serial.print(" MG1_S: ");
Serial.print(inverterStatus.mg1Speed);
Serial.print(" MG2_S: ");
Serial.println(inverterStatus.mg2Speed);
// Data
Serial.print("Incomming packages: ");
Serial.print(inCounter);
Serial.print(" Outgoing packages: ");
Serial.println(outCounter);
}
}