openinverter forum

Code: Select all

// Change log
/*

1_19
* Changed torqueLimit to 125 (100) and powerLimit to 30 (25)
* Changed brakeLightTorqueOn to -15 (-20)

1_18
* Changed torqueLimit to 100 (40) and powerLimit to 25 (20) 

1_17
* Declare powerLimitedAccTorque as int16_t (uint16_t) and changes accordingly in limitedTorqueRequest()
* Changed torqueLimit to 40 (100) and powerLimit to 20 (30) 

1_16
* Changed how powerLimitedAccTorque is calculated in limitedTorqueRequest()
* Set powerLimitedAccTorque active

1_15
* Returned change of id 0x1D4 byte5 and 0x11A byte0 when changing gear, in sendPTCAN()
* Added maxSpeedGearChange constant and not allowing gear change above this speed in requestGear()

1_14
* Changed ID: 1D4, byte 6 0x01 (0x30), in sendPTCAN()

1_13
* Added DCDC_ctrl to work with HMI_10 HW
* Removed unused Dipped beam to free a IO PIN

1_12
* Changes to readPTCAN() to make motor current have the correct representation

1_11
* Changes to readPTCAN() to make motor current and voltage have the correct representation
* Added sendEnd() to reduce number of code lines

1_10
* Changes to sendSerialLCDPageTemp() to work with LCD_8_0
* Changed to checkInputVoltage() to return uin8_t value (boolean)
* Changes to sendSerialLCDPageMain() and sendSerialLCDPageTemp() to work with changed output from checkInputVoltage()
* Changed lowInputVoltageThreshold to 143 (144)

1_9
* Changed levels for regen ramp down and brake light

1_8
* Added regen torque command and other changed to limitedTorqueRequest()
* Moved gear selection to limitedTorqueRequest() from sendPTCAN()
* Added motorDirection to inverterStatus struct
* Added two limiting constants minimumRegenRpm and rampRegenRpm
* Changed how gear is used to singed value ("char")
* Added control of brake light to limitedTorqueRequest()
* Removed brakeLight()

1_7
* Corrected id 0x1D4 byte0 to 0x6E (0x7F) and byte1 to 0x6E (0x07) to be valid for the gen2 inverter

1_6
* Removed change of id 0x1D4 byte5 and 0x11A byte0 when changing gear, only changing sign of requested torque, in sendPTCAN()
* Changed how gear is used to "char" of selected gear in requestGear(), sendSerialLCDPageMain() and sendPTCAN()

1_5
* Removed message 0x1DB from sendPTCAN()
* Sending 0x50B with 100ms (10ms) interval in sendPTCAN()
* Made pedalPosition a global variable and changed how sendSerialLCDPageMain() sends the pedal
* Changed order of CAN frames in sendPTCAN()
* Added changed of id 0x1D4 byte5 when changing gear

1_4
* Changed motor rpm to signed value and then added abs() to get unsigned value in readPTCAN()

1_3
* Changed sign on torque command when in reverse gear in sendPTCAN()
* Removed regen torque and power limited torque from limitedTorqueRequest()
* Simplified calculation of sent torque request in limitedTorqueRequest() and sendPTCAN()
* Added division by 2 for motor RPM in readPTCAN()

1_2
* Made gear a static variable in requestGear()

1_1
* Adjusted requestGear() to start in neutral (0x3E)

1_0
* Initial version based on HMI2_2_10
* Added second CAN-bus to control Leaf inverter

*/

// Include libraries
#include "mcp_can.h"
#include "SPI.h"
#include "EEPROM.h"

// Constant declaration
// Button state MASK
const uint8_t State_BPA = B00000001;
const uint8_t State_BPD = B00001000;
const uint8_t State_BPR = B00010000;

// statusPBC MASK
const uint8_t statusON					= 0x01;
const uint8_t statusOFF					= 0x02;
const uint8_t statusRunningON 	= 0x04;
const uint8_t statusRunningOFF	= 0x08;

// Limits
const uint16_t pot1Min = 130; // Min limit for pot1
const uint16_t pot1Max = 950;	// Max limit for pot1
const uint16_t pot2Min = 50; // Min limit for pot2
const uint16_t pot2Max = 485; // Max limit for pot2
const uint8_t diffError = 50; // Max diff between pot1 and pot2 to trigger error handling
// Pedal map 
const uint16_t potNomMin = 152; // For calculating pedal map
const uint16_t potNomMax = 927; // For calculating pedal map
const uint16_t potLimp = 475; // 40% of max positive torque (0.4 * 500 + 275)
// Input voltage limit constants
const uint8_t lowInputVoltageThreshold = 143; // Voltage level (10,0V) to trigger odometer save to EEPROM, (10,0 * 1024 * 47 / (5 * (120+47)) - 0.5) / 4
const uint8_t lowInputVoltageWarning = 190; // Voltage level (13,2V) to trigger 12V warning light, (13,2 * 1024 * 47 / (5 * (120+47)) - 0.5) / 4
// Powertrain limits
const uint8_t torqueLimit = 125; // Maximum torque limit (Nm)
const uint8_t powerLimit = 30; // Maximum power limit (kW)
const uint8_t minimumRegenRpm = 40; // No regen torque below this motor speed (rpm)
const uint8_t rampRegenRpm = 240; // Start ramping down regen torque below this motor speed (rpm)
const uint8_t maxSpeedGearChange = 5; // No gear change allowed above this speed
// Brake light settings
const int8_t brakeLightTorqueOn = -15; // Torque to turn on brake light (Nm)
const int8_t brakeLightTorqueOff = -10; // Torque to turn off brake light (Nm)


// Variable declaration
//uint8_t inverter[8];
uint8_t mainBMS[8];
uint8_t highLowBMS[8];
uint8_t tempBMS[8];
uint16_t cellsBMS[42];
boolean newRevsDataFlag = false;
uint8_t statusPBC;
uint16_t pedalPosition = 0;

struct InverterStatus {
	uint16_t voltage = 0;
	int16_t current = 0;
	int8_t motorDirection = 0;
	uint16_t motorSpeed = 0;
	boolean error = false;
	int8_t invTemp = 0;
	int8_t motorTemp = 0;
} inverterStatus;

// I/O-PINS
const uint8_t SPI_CS_PIN = 10; // D10, CS PIN for MCP2515 CAN module
const uint8_t SPI_PTCS_PIN = 17; // A3, CS PIN for MCP2515 CAN module
const uint8_t buttonAPIN = 4; // Stalk button
const uint8_t buttonDPIN = 9; // Drive button
const uint8_t buttonRPIN = 8; // Reverse button
const uint8_t brakeLightPIN = 5; // Brake light relay
const uint8_t brakeFluidLevelPIN = 7; // Brake fluid level switch indicator
const uint8_t parkBrakePIN = 6; // Park brake status indicator
const uint8_t tsRPIN = 2; // Right turn signal indicator
const uint8_t tsLPIN = 3; // Left turn signal indicator
const uint8_t positionLightPIN = 14; // A0, Position light indicator
const uint8_t pot1PIN = 15; // A1, pot1 from accelerator pedal
const uint8_t pot2PIN = 16; // A2, pot2 from accelerator pedal
const uint8_t dcdcCtrlPIN = 19; // A5, To control DCDC turn on 
const uint8_t mainBeamPIN = 18; // A4, Main beam indicator
const uint8_t rawVoltageInputPIN = 6; // A6 for analogRead()
const uint8_t carVoltageInputPIN = 7; // A7 for analogRead()


MCP_CAN CAN(SPI_CS_PIN);	// Set CS pin
MCP_CAN PTCAN(SPI_PTCS_PIN); // Set CS pin


/********
* SETUP *
********/
void setup()
{
	pinMode(buttonAPIN, INPUT_PULLUP);
	pinMode(buttonDPIN, INPUT_PULLUP);
	pinMode(buttonRPIN, INPUT_PULLUP);
	pinMode(brakeFluidLevelPIN, INPUT_PULLUP);
	pinMode(parkBrakePIN, INPUT_PULLUP);
	pinMode(tsRPIN, INPUT_PULLUP);
	pinMode(tsLPIN, INPUT_PULLUP);
	pinMode(positionLightPIN, INPUT_PULLUP);
	pinMode(mainBeamPIN, INPUT_PULLUP);
		
  pinMode(brakeLightPIN, OUTPUT);
  digitalWrite(brakeLightPIN, LOW);
  
  pinMode(dcdcCtrlPIN, OUTPUT);
  digitalWrite(dcdcCtrlPIN, LOW);
  
	Serial.begin(115200);
	
	initCAN();
	
	delay(100);
}


/*******
* LOOP *
*******/
void loop()
{
  static uint32_t lastMillisSerialLCD = millis(); // For timing
  static uint8_t page = 0;
  static uint32_t savedOdometer = 0;
  static uint16_t savedTripmeter = 0;
  static uint16_t resetTripmeter = 0;
  static bool readFlag = false;
  
  if(!readFlag)
  {
		readFlag = true;
    
		for(int i=0; i<4; i++)
		{
			uint32_t odo = EEPROM.read(i);
			savedOdometer |= (odo << (i*8));
		}

    for(int i=0; i<2; i++)
    {
      uint16_t trip = EEPROM.read(i+4);
      savedTripmeter |= (trip << (i*8));
    }
    
	}
    
  readCAN();
  readPTCAN();
	uint8_t buttonState = readButtons();
	uint8_t carSpeed = revsToSpeed();
	int8_t gear = requestGear(buttonState, carSpeed);
	sendPTCAN(gear);
	sendCAN();
	turnSignal();
	
	uint16_t tripmeter = speedToTrip(carSpeed);
	uint16_t tripmeter2 = savedTripmeter + tripmeter - resetTripmeter;
	uint32_t odometer = savedOdometer + tripmeter;
	
	saveOdometerToEEPROM(odometer, tripmeter2);
	
	// Reset tripmeter before 1000km is reached
	(tripmeter2 >= 10000) ? (resetTripmeter = savedTripmeter + tripmeter) : 0;
		
	uint8_t newPage = changePageOrReset(page, buttonState);
	
	// Check if reset is requested, if not update page
	(newPage == 0xFF) ? (resetTripmeter = savedTripmeter + tripmeter) : page = newPage;
		
	if (millis() - lastMillisSerialLCD >= 15) // Test if the 15ms period has elapsed
  {
		lastMillisSerialLCD = millis();  // IMPORTANT to save the start time
		sendSerialLCD(page, carSpeed, tripmeter, tripmeter2, odometer, gear);
	}
}


/*****************************************************************************************
* Checks CAN-bus buffer and if new data is available stores it in appropriate data array *
*****************************************************************************************/
void readCAN()
{
	uint32_t rxId;
  uint8_t len;
  uint8_t rxBuf[8];
  static boolean bmsAlive = false;

  if(CAN_MSGAVAIL == CAN.checkReceive()) // Check if data is coming
  {
    CAN.readMsgBuf(&rxId, &len, rxBuf); // read data, rxId: message ID, ext: flag extended ID, len: data length, rxBuf: data buf
    
    if(rxId == 0x100) // Main data from BMS
    {
      for(int i=0; i<8; i++)
      {
				mainBMS[i] = rxBuf[i];
			}
			if(!bmsAlive)
			{
				bmsAlive = true;
				digitalWrite(dcdcCtrlPIN, HIGH);
			}
    }
		if(rxId == 0x101) // Highest and lowest cell data from BMS
    {
      for(int i=0; i<8; i++)
      {
				highLowBMS[i] = rxBuf[i];
			}
    }
		if(rxId == 0x102) // Temperature data from BMS
    {
      for(int i=0; i<8; i++)
      {
				tempBMS[i] = rxBuf[i];
			}
    }
    if(rxId == 0x091) // PBC status
    {
			statusPBC = rxBuf[0];
		}
  }
}


/*********************************************
* Send park brake request to PBC via CAN-bus *
*********************************************/
void sendCAN()
{
	static uint32_t lastMillisCAN = millis();
	
	if (millis() - lastMillisCAN >= 100) // Test if the 100ms period has elapsed
  {
    lastMillisCAN = millis(); // reset timer
    
    uint8_t parkBrakeRequest[1];
    if(!digitalRead(parkBrakePIN)) // Pulled up
    {
			parkBrakeRequest[0] = 0x01; // requestON
		}
		else
		{
			parkBrakeRequest[0] = 0x02; // requestOFF
		}
		
		CAN.sendMsgBuf(0x090, 0, 1, parkBrakeRequest); // Send data to PBC
	}
}


/*****************************************************
* Reads pot data, check for errors and set potReturn *
*****************************************************/
int16_t readPot()
{
	uint16_t pot1 = analogRead(pot1PIN); // Read accelerator pot1
	uint16_t pot2 = analogRead(pot2PIN); // Read accelerator pot2
	
  int16_t potReturn;
  static bool potGood = false;
  
  potGood = potChecker(pot1, pot2);
  
	if (potGood) // Pot:s good, set output from pot1
	{
		potReturn = pedalMap(pot1, false); // Set output value from pot1	
	}
	else // Pot:s bad, check whats causing the error and possibly allow limp home
	{
		if ((pot1 < pot1Min || pot1 > pot1Max) && (pot2 > pot2Min && pot2 < pot2Max)) // Check if pot1 is causing error
		{
			potReturn = pedalMap(pot2*2, true); // Set output value from pot2
		}
		else if ((pot2 < pot2Min || pot2 > pot2Max) && (pot1 > pot1Min && pot1 < pot1Max)) // Check if pot2 is causing error
		{
			potReturn = pedalMap(pot1, true); // Set output value from pot1
		}
		else if ((pot1 < pot1Min || pot1 > pot1Max) && (pot2 < pot2Min || pot2 > pot2Max)) // Check if both pot:s are outside nominal range
		{
			potReturn = 0; // Stand still
		}
		else // Some other fault, could be a loose wire or a faulty potentiometer causing values to float around
		{
			uint16_t pot = min(pot1, (pot2*2));
			potReturn = pedalMap(pot, true); // Set output from min of pot1 and pot2
		}
	}
	
	return potReturn;
}


/****************************************************************
* Checks if pot:s are OK to use and starts error counter if not *
****************************************************************/
bool potChecker(uint16_t pot1, uint16_t pot2)
{
	static uint8_t errorCounter = 0;
	static uint8_t worksCounter = 5; // Assume pot:s are good at start
	static bool potGood = false;
	int16_t diff = pot1 - (2*pot2);
	
	bool potError = abs(diff) > diffError || pot1 < pot1Min || pot1 > pot1Max || pot2 < pot2Min || pot2 > pot2Max; // Checks if pot:s agree and are within limits, if not set error
	
	if (potError)
  {
    worksCounter = 0;
    if (errorCounter < 5)
    {
      errorCounter++;
    }
    else
    {
      potGood = false;
    }
  }
  else
  {
    errorCounter = 0;
    if (worksCounter < 5)
    {
      worksCounter++;
    }
    else
    {
      potGood = true;
    }
  }

  return potGood;	
}


/*************************************************
* Maps output to controller from pedal pot input *
*************************************************/
int16_t pedalMap(uint16_t potInput, bool limpHome)
{
	int16_t potReturn; // (0 - 775)
	static bool limpHomeTriggered = false;
	
	if (potInput < potNomMin) // To avoid values outside nominal values
  {
    potInput = potNomMin;
  }
	if (potInput > potNomMax) // To avoid values outside nominal values
  {
    potInput = potNomMax;
  }
  
  potReturn = potInput - potNomMin;
  
  if (limpHome || limpHomeTriggered) // Check if limpHome is triggered
	{
		limpHomeTriggered = true;
		if (potReturn > potLimp)
		{
			potReturn = potLimp;
		}
	}
	
	return potReturn;
}


/*************************************************************
* Checks if any buttons are pressed and returns button state *
*************************************************************/
uint8_t readButtons()
{
	static uint32_t lastMillisButton;
	static uint8_t lastButtonState;
	static uint8_t buttonState;
	uint8_t buttonReading;
	
	buttonReading = (digitalRead(buttonAPIN) == LOW) ? State_BPA : 0;
	buttonReading |= (digitalRead(buttonDPIN) == LOW) ? State_BPD : 0;
	buttonReading |= (digitalRead(buttonRPIN) == LOW) ? State_BPR : 0;
		
	(buttonReading != lastButtonState) ? (lastMillisButton = millis()) : 0;	// reset timer
	
	(millis() - lastMillisButton > 100) ? (buttonState = buttonReading) : 0; // 100ms
	
	lastButtonState = buttonReading;

	return buttonState;
}	


/*****************************************************************
* Reads button state and returns requested page or reset request *
*****************************************************************/
uint8_t changePageOrReset(uint8_t page, uint8_t buttonState)
{
	static uint32_t lastMillisButtonA;
	static uint8_t buttonAPressedFlag = 0;

  if((buttonState & State_BPA) && (buttonAPressedFlag == 0)) // Button A depressed and has not been depressed before
  {
    lastMillisButtonA = millis(); // Reset the timer
    buttonAPressedFlag = 1;
  }
  if((buttonState & State_BPA) && (buttonAPressedFlag == 1) && (millis() - lastMillisButtonA > 3000)) // 3s
  {
		buttonAPressedFlag = 2;
		page = 0xFF; // Request reset
	}
  if(((buttonState & State_BPA) == 0) && (buttonAPressedFlag != 0)) // Button A released after having been depressed
  {
		if(buttonAPressedFlag == 1)
    {
      (page >= 1) ? page = 0 : page++; 
    }
		
    buttonAPressedFlag = 0;
  }
  
	return page;
}


/***********************************************
* Reads button state and returns selected gear *
***********************************************/
int8_t requestGear(uint8_t buttonState, uint8_t carSpeed)
{
	static int8_t gear = 0;
	
	if((buttonState & State_BPD) && !(buttonState & State_BPR) && (carSpeed < maxSpeedGearChange))
	{
		gear = 1; // Drive
	}
	if((buttonState & State_BPR) && !(buttonState & State_BPD) && (carSpeed < maxSpeedGearChange))
	{
		gear = -1; // Reverse
	}
	if((buttonState & State_BPD) && (buttonState & State_BPR))
	{
		gear = 0; // Neutral
	}

	return gear;
}


/*****************************************************************************************************************
* Measures car 12V-system voltage, calculates average over 5 values and turns on or off 12V system warning light *
*****************************************************************************************************************/
uint8_t checkInputVoltage()
{
	static uint8_t inputVoltageArray[5] = {0, 0, 0, 0, 0};
	static uint8_t readIndex = 0;
	uint16_t sumOfReadings = 0;
	
	uint8_t voltage = analogRead(carVoltageInputPIN) >> 2; // Division by 4
	inputVoltageArray[readIndex] = voltage;
	readIndex++;
	
	if(readIndex >= 5)
	{
		readIndex = 0;
	}
		
	for(int i=0; i<5; i++)
	{
		sumOfReadings += inputVoltageArray[i];
	}
	
	uint8_t inputVoltage = sumOfReadings / 5;
	
	//boolean status12VError = inputVoltage < lowInputVoltageWarning; // Set 12V warning light if triggered
	
	return inputVoltage;
}


/****************************************************************************************
* Reads arduino raw input voltage and saves odometer to EEPROM if below threshold value *
****************************************************************************************/
void saveOdometerToEEPROM(uint32_t odometerToSave, uint16_t tripmeterToSave)
{
	static boolean savedFlag = false;
	static uint32_t lastMillisSave = millis();
	
	uint8_t inputVoltage = analogRead(rawVoltageInputPIN) >> 2; // Division by 4
  
	if((inputVoltage < lowInputVoltageThreshold) && !savedFlag) // Save odometer if input voltage is below threshold and not saved before
	{
		savedFlag = true;
		lastMillisSave = millis(); // Reset timer
		
		for(int i=0; i<4; i++)
		{
			uint8_t odo = (odometerToSave >> (i*8)) & 0xFF;
			EEPROM.update(i, odo);
		}
		
		EEPROM.update(4, lowByte(tripmeterToSave));
		EEPROM.update(5, highByte(tripmeterToSave));
	}
	
	if ((millis() - lastMillisSave >= 10000) && savedFlag) // Test if the 10s period has elapsed
  {
    lastMillisSave = millis(); // Reset timer
    savedFlag = false; // Reset flag
	}
}


/***************************************************************************************************************************************
* Checks if new motor revolution data is available, calculates average over 5 values, translates it to car speed and returns car speed *
***************************************************************************************************************************************/
uint8_t revsToSpeed()
{
	uint8_t carSpeed;
	static uint16_t motorRevsInput[5] = {0, 0, 0, 0, 0};
	static uint8_t readIndex = 0;
	uint16_t sumOfReadings = 0;
			
	if(newRevsDataFlag)
	{
		newRevsDataFlag = false;
		
		motorRevsInput[readIndex] = inverterStatus.motorSpeed;
		readIndex++;
	
		if(readIndex >= 5)
		{
			readIndex = 0;
		}
	}
		
	for(int i=0; i<5; i++)
	{
		sumOfReadings += motorRevsInput[i];
	}
	
	carSpeed = sumOfReadings / 385; // Car speed in km/h (77 * 5 = 385)
	
	return carSpeed;
}


/*******************************************************************************************************
* Calculates the distance traveled by integrating the car speed, returns distance with 100m resolution *
*******************************************************************************************************/
uint32_t speedToTrip(uint8_t carSpeed)
{
	static uint32_t distanceIn100Meter = 0;
	static uint32_t distanceInMeter3600 = 0;
	static uint32_t lastMillisTrip = millis(); // For timing
	uint16_t timeSinceLast;
		
	if (millis() - lastMillisTrip >= 100) // Test if the 100ms period has elapsed
  {
		timeSinceLast = millis() - lastMillisTrip;
		lastMillisTrip = millis(); // Reset timer
    
    distanceInMeter3600 += timeSinceLast * carSpeed;
    
    if(distanceInMeter3600 >= 360000) // Translate m*3600 to 100m
    {
			distanceIn100Meter++;
			distanceInMeter3600 -= 360000; 
		}
	}
	
	return distanceIn100Meter;
}


/*********************************************************************
* Calculates the average battery current over the last five readings *
*********************************************************************/
int16_t averageBatteryCurrent()
{
	static int16_t batteryCurrentArray[5] = {0, 0, 0, 0, 0};
	static uint8_t readIndex = 0;
	int16_t sumOfReadings = 0;
	
	int16_t current = (mainBMS[0] << 8) + mainBMS[1]; // Battery pack current
	batteryCurrentArray[readIndex] = current;
	readIndex++;
	
	if(readIndex >= 5)
	{
		readIndex = 0;
	}
		
	for(int i=0; i<5; i++)
	{
		sumOfReadings += batteryCurrentArray[i];
	}
	
	int16_t averageBatteryCurrent = sumOfReadings / 5;
	
	return averageBatteryCurrent;
}


/****************************************
* Sends end transmission command to LCD *
****************************************/
void sendEnd()
{
	Serial.write(0xFF);
	Serial.write(0xFF);
	Serial.write(0xFF);
}


/*************************************************************************************
* Determines which page to display on LCD and calls corresponding data send function *
*************************************************************************************/
void sendSerialLCD(uint8_t page, uint8_t carSpeed, uint16_t tripmeter, uint16_t tripmeter2, uint32_t odometer, int8_t gear)
{
	static uint8_t pageOld = 1; // Set to !=0 to force page change at startup
	uint8_t inputVoltage = checkInputVoltage();	
	
	if(page == 0)
	{
		if(page != pageOld)
		{
			sendEnd();
			
			Serial.print("page m");
			sendEnd();
      
			pageOld = page;
		}
		else
		{
			sendSerialLCDPageMain(carSpeed, tripmeter2, gear, inputVoltage);
		}
	}
	if(page == 1)
	{
		if(page != pageOld)
		{
      sendEnd();
      
      Serial.print("page t");
			sendEnd();
      
			pageOld = page;
		}
		else
		{
			sendSerialLCDPageTemp(tripmeter, tripmeter2, odometer, inputVoltage);
		}
	}
}


/**********************************************************
* Page Main, calculates and sends LCD data via serial bus *
**********************************************************/
void sendSerialLCDPageMain(uint8_t carSpeed, uint16_t tripmeter2, int8_t gear, uint8_t inputVoltage)
{
	static uint8_t counter = 0;
	
	sendEnd();
	
	uint16_t pedalColour = 65535; // White	
	uint8_t pedalValue;
	
	if(pedalPosition < 250) // Regen
	{
		pedalValue = map(pedalPosition, 0, 250, 0, 30);
		pedalColour = 31; // Blue (regen)
	}
	else if(pedalPosition > 275) // Throttle
	{
		pedalValue = map(pedalPosition, 275, 775, 30, 100);
		pedalColour = 2016; // Green (throttle)
	}
	else // Coast
	{
		pedalValue = 30;
		pedalColour = 65535; // White
	}
	
	Serial.print("m.pd.val=");
	Serial.print(pedalValue);
	sendEnd();
	
	Serial.print("m.pd.pco=");
	Serial.print(pedalColour);
	sendEnd();
	
	
	if(counter == 0)
	{
		Serial.print("m.sp.val=");
		Serial.print(carSpeed);
		sendEnd();
		
		if(digitalRead(positionLightPIN))
    {
      Serial.print("vis po,0");
      sendEnd();
    }
    else
    {
      Serial.print("vis po,1"); // Position light
      sendEnd();
    }
		
		counter++;
	}
	else if(counter == 1)
	{
		uint16_t capacity = (mainBMS[4] << 8) + mainBMS[5]; // Battery pack remaining capacity
		uint8_t valCapacity;
	
		if(capacity >= 50)
		{
			valCapacity = ((capacity - 50) * 100) / 198; // Battery capacity in % of 19.8Ah (24.8Ah * 80%)
		}
		else
		{
			valCapacity = 0;
		}
	
		Serial.print("m.ca.val=");
		Serial.print(valCapacity);
		sendEnd();
		
		if(digitalRead(mainBeamPIN))
    {
      Serial.print("vis mb,0");
      sendEnd();
    }
    else
    {
      Serial.print("vis mb,1"); // Main beam
      sendEnd();
    }
		
		counter++;
	}
	else if(counter == 2)
	{
		Serial.print("m.ta.val=");
		Serial.print(tripmeter2);
		sendEnd();
		
		static uint8_t pa;
		if(statusPBC == statusOFF)
    {
      pa = 0;
    }
    if(statusPBC == statusON)
    {
      pa = 1;
    }
    if(statusPBC == statusRunningON || statusPBC == statusRunningOFF)
    {
			static uint8_t blinkCounter = 0;
			blinkCounter++;
			if(blinkCounter > 3)
			{
				blinkCounter = 0;
				pa = (pa == 0) ? 1 : 0;
			}
		}
		
		Serial.print("vis pa,");
    Serial.print(pa);
    sendEnd();
				
		counter++;
	}
	else if(counter == 3)
	{
		uint8_t gearSet;
		if(gear == 0) // Neutral
		{
			gearSet = 'N';
		}
		else if(gear == -1) // Reverse
		{
			gearSet = 'R';
		}
		else if(gear == 1) // Drive
		{
			gearSet = 'D';
		}
		else
		{
			gearSet = 'P'; // error (P)
		}
	
		Serial.print("m.ge.txt=");
		Serial.write(0x22);
		Serial.write(gearSet);
		Serial.write(0x22);
		sendEnd();
		
		if(digitalRead(brakeFluidLevelPIN))
    {
      Serial.print("vis bf,0");
      sendEnd();
    }
    else
    {
      Serial.print("vis bf,1"); // Brake fluid level
      sendEnd();
    }
		
		counter++;
	}
	else if(counter == 4)
	{
		uint16_t totalVoltage = (mainBMS[2] << 8) + mainBMS[3]; // Battery pack total voltage
		
		Serial.print("m.vo.val=");
		Serial.print(totalVoltage);
		sendEnd();
		
		if(inputVoltage < lowInputVoltageWarning)
    {
      Serial.print("vis bl,1"); // Low 12V battery voltage
      sendEnd();
    }
    else
    {
      Serial.print("vis bl,0");
      sendEnd();
    }
		
		counter++;
	}
	else if(counter == 5)
	{
		int16_t batteryCurrent = averageBatteryCurrent();
		String stringCurrent;
		stringCurrent = ((batteryCurrent < 100) && (batteryCurrent > -100)) ? String(batteryCurrent / 10.0, 1) : String(batteryCurrent / 10);
			
		Serial.print("m.cu.txt=");
		Serial.write(0x22);
		Serial.print(stringCurrent);
		Serial.write(0x22);
		sendEnd();
		
		if(inverterStatus.invTemp > 60 || inverterStatus.motorTemp > 60)
    {
      Serial.print("vis ot,1"); // Over temperature
      sendEnd();
    }
    else
    {
      Serial.print("vis ot,0");
      sendEnd();
    }
		
		counter++;
	}
	else if(counter == 6)
	{
		if(mainBMS[6] & 0x02)
    {
      Serial.print("vis bh,1"); // Low main battery voltage
      sendEnd();
    }
    else
    {
      Serial.print("vis bh,0");
      sendEnd();
    }
		
		
		if((mainBMS[6] & 0x04) || inverterStatus.error)
		{
      Serial.print("vis mw,1"); // Master warning
      sendEnd();
    }
    else
    {
      Serial.print("vis mw,0");
      sendEnd();
    }
		
		counter = 0;
	}
	else // error handler
	{
		counter = 0;
	}
}


/**********************************************************
* Page Temp, calculates and sends LCD data via serial bus *
**********************************************************/
void sendSerialLCDPageTemp(uint16_t tripmeter, uint16_t tripmeter2, uint32_t odometer, uint8_t inputVoltage)
{
	static uint8_t counter = 0;
	
	sendEnd();
	
	if(counter == 0)
	{
		Serial.print("t.mv.val=");
		Serial.print(inverterStatus.voltage);
		sendEnd();
		
		Serial.print("t.mc.val=");
		Serial.print(inverterStatus.current);
		sendEnd();
		
		uint8_t batteryVoltage = inputVoltage * 0.694; // 5 * (120 + 47) * 4 * 10 / (1024 * 47)
		
		Serial.print("t.bv.val=");
		Serial.print(batteryVoltage);
		sendEnd();
		
		counter++;
	}
	else if(counter == 1)
	{
		Serial.print("t.od.val=");
		Serial.print(odometer / 10);
		sendEnd();
	
		Serial.print("t.ta.val=");
		Serial.print(tripmeter2);
		sendEnd();
	
		Serial.print("t.tb.val=");
		Serial.print(tripmeter);
		sendEnd();
		
		counter++;
	}
	else if(counter == 2)
	{
		int8_t tempModule = tempBMS[0]; // Lowest cell module temperature in degree C
        
    Serial.print("t.t0.val=");
    Serial.print(tempModule);
    sendEnd();

    tempModule = tempBMS[1]; // Highest cell module temperature in degree C
        
    Serial.print("t.t1.val=");
    Serial.print(tempModule);
    sendEnd();

    tempModule = tempBMS[2]; // Battery box internal temperature in degree C
        
    Serial.print("t.t2.val=");
    Serial.print(tempModule);
    sendEnd();

    tempModule = tempBMS[3]; // BMS slave module highest die temperature in degree C
        
    Serial.print("t.t3.val=");
    Serial.print(tempModule);
    sendEnd();
		
		counter++;
	}
	else if(counter == 3)
	{
		uint8_t highCell = highLowBMS[0]; // Highest cell number
		uint16_t highVoltage = (highLowBMS[1] << 8) + highLowBMS[2]; // Highest cell voltage
	
		Serial.print("t.hc.val=");
		Serial.print(highCell);
		sendEnd();
	
		Serial.print("t.hv.val=");
		Serial.print(highVoltage);
		sendEnd();
	
		uint8_t lowCell = highLowBMS[3]; // Lowest cell number
		uint16_t lowVoltage = (highLowBMS[4] << 8) + highLowBMS[5]; // Lowest cell voltage
	
		Serial.print("t.lc.val=");
		Serial.print(lowCell);
		sendEnd();
	
		Serial.print("t.lv.val=");
		Serial.print(lowVoltage);
		sendEnd();
		
		counter++;
	}
	else if(counter == 4)
	{
		Serial.print("t.s0.val=");
		Serial.print(mainBMS[6]);
		sendEnd();
		
		Serial.print("t.tm.val=");
		Serial.print(inverterStatus.motorTemp);
		sendEnd();
		
		Serial.print("t.ti.val=");
		Serial.print(inverterStatus.invTemp);
		sendEnd();
			
		uint16_t capacity = (mainBMS[4] << 8) + mainBMS[5]; // Battery pack remaining capacity
		
		Serial.print("t.ca.val=");
		Serial.print(capacity);
		sendEnd();
		
		counter = 0;
	}
	else // error handler
	{
		counter = 0;
	}	
}


/*****************************************************
* Sends turn signal indication to LCD via serial bus *
*****************************************************/
void turnSignal()
{
	static boolean turnSignalRightOld = false;
	static boolean turnSignalLeftOld = false;
	
	boolean turnSignalLeft = !digitalRead(tsLPIN);
	boolean turnSignalRight = !digitalRead(tsRPIN);
	
	if(turnSignalRight != turnSignalRightOld)
	{
		sendEnd();
		
		if(turnSignalRight)
		{
			turnSignalRightOld = true;
			
			Serial.print("vis r,1");
			sendEnd();
		}
		else
		{
			turnSignalRightOld = false;
			
			Serial.print("vis r,0");
			sendEnd();
		}
	}
	
	if(turnSignalLeft != turnSignalLeftOld)
	{
		sendEnd();
		
		if(turnSignalLeft)
		{
			turnSignalLeftOld = true;
			
			Serial.print("vis l,1");
			sendEnd();
		}
		else
		{
			turnSignalLeftOld = false;
			
			Serial.print("vis l,0");
			sendEnd();
		}
	}
}


/*********************************
* Reads PTCAN data from inverter *
*********************************/
void readPTCAN()
{
	uint32_t rxId;
  uint8_t len;
  uint8_t rxBuf[8];  
        
  if(CAN_MSGAVAIL == PTCAN.checkReceive()) // Check if data is coming
  {
    PTCAN.readMsgBuf(&rxId, &len, rxBuf); // read data, rxId: message ID, ext: flag extended ID, len: data length, rxBuf: data buf
    
    if(rxId == 0x1DA) // Status from Inverter
    {
			uint16_t parsedVoltage = (rxBuf[0] << 2) | (rxBuf[1] >> 6); // Inverter voltage (2 x V)
			inverterStatus.voltage = parsedVoltage / 2; // Inverter voltage (V)
			uint16_t parsedCurrent = ((rxBuf[2] & 0x07) << 8) | rxBuf[3]; // Inverter current (A), 11-bit
			inverterStatus.current = (int16_t)(parsedCurrent << 5) >> 5; // Inverter current (A), 16-bit signed
			int16_t parsedSpeed = (rxBuf[4] << 8) | rxBuf[5];
			int16_t speed = (parsedSpeed == 0x7FFF ? 0 : parsedSpeed); // Motor speed (2 x rpm)
			inverterStatus.motorDirection = speed < 0 ? -1 : 1; // Motor rotation direction (1 is forward, -1 is reverse)
			inverterStatus.motorSpeed = abs(speed) / 2; // Absolute value of motor speed (rpm)
			inverterStatus.error = (rxBuf[6] & 0xB0) != 0x00; // Inverter error if true
			newRevsDataFlag = true;
    }
    
    if(rxId == 0x55A) // Temperature from Inverter
    {
			inverterStatus.invTemp = fahrenheitToCelsius(rxBuf[2]); // Inverter temperature (degree C)
			inverterStatus.motorTemp = fahrenheitToCelsius(rxBuf[1]); // Motor temperature (degree C)
    } 
  }  
}


/***************************************************
* Sends control commands to inverter via PTCAN-bus *
***************************************************/
void sendPTCAN(int8_t gear)
{
	static uint32_t lastMillisPTCAN = millis();	
	
	if (millis() - lastMillisPTCAN >= 10) // 10ms
  {
    lastMillisPTCAN = millis(); // reset timer
    static uint8_t counter = 0;
    static uint8_t counter50B = 0;
    counter50B++;
    
    uint8_t byte1D4B5;
    uint8_t byte11AB0;
		if(gear == 0) // Neutral
		{
			byte1D4B5 = 0x43;
			byte11AB0 = 0x3E;
		}
		else if(gear == -1) // Reverse
		{
			byte1D4B5 = 0x42;
			byte11AB0 = 0x2E;
		}
		else if(gear == 1) // Drive
		{
			byte1D4B5 = 0x44;
			byte11AB0 = 0x4E;
		}
		else
		{
			byte1D4B5 = 0x00;
			byte11AB0 = 0x00;
		}
    
    // 1D4
    uint8_t bytes1D4[8] = {0x6E, 0x6E, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00};
    
    int16_t torqueRequest = limitedTorqueRequest(gear);		
    bytes1D4[2] = highByte(torqueRequest);
    bytes1D4[3] = lowByte(torqueRequest) & 0xF0;
    
    bytes1D4[4] = 0x07 | (counter << 6);
    bytes1D4[5] = byte1D4B5;
    nissanCRC(bytes1D4);
    
    PTCAN.sendMsgBuf(0x1D4, 0, 8, bytes1D4);
    
    // 50B
    if(counter50B > 9) // every 100ms
		{
			counter50B = 0; // reset counter
			uint8_t bytes50B[7] = {0x00, 0x00, 0x06, 0xC0, 0x00, 0x00, 0x00};
    
			PTCAN.sendMsgBuf(0x50B, 0, 7, bytes50B);
		}
    
		// 11A
    uint8_t bytes11A[8] = {0x00, 0x40, 0x00, 0xAA, 0XC0, 0x00, 0x00, 0x00};
    
    bytes11A[0] = byte11AB0;
    bytes11A[6] = counter;
    nissanCRC(bytes11A);
    
    PTCAN.sendMsgBuf(0x11A, 0, 8, bytes11A);
    
    // Counter
    counter++;
    if(counter > 3)
    {
      counter = 0;
    }
	}
}


/******************************************************
* Calculates what torque to request from the inverter *
******************************************************/
int16_t limitedTorqueRequest(int8_t gear)
{
	uint16_t potRequest = readPot(); // (0 - 775)
	pedalPosition = potRequest; // For LCD presentation of pedal position
	int16_t torqueRequest = 0;
	int16_t accTorqueRequestLimit = torqueLimit * 72; // 250Nm * 72 = 18000 
	int16_t regenTorqueRequestLimit = -(accTorqueRequestLimit / 2); // Acc torque / 2
	
  int16_t powerLimitedAccTorque = 0x7FFF;
  if(inverterStatus.motorSpeed != 0)
  {
		powerLimitedAccTorque = min(((int32_t)powerLimit * 687549 / inverterStatus.motorSpeed), (int32_t)0x7FFF); // 1000 x 60 x 72 / (2 x pi)
	}
  int16_t powerLimitedRegenTorque = -(powerLimitedAccTorque / 2); // Acc torque / 2
  
  boolean gearSelected = abs(gear) == 1; // Drive or reverse selected
  static boolean brakeLightOn = false;
  	
	if(gearSelected && ((potRequest >= 275) && (potRequest <= 775))) // Acceleration torque requested
	{
		torqueRequest = map(potRequest, 275, 775, 0, accTorqueRequestLimit);
		
		if(torqueRequest > powerLimitedAccTorque)
		{
			torqueRequest = powerLimitedAccTorque;
		}
		
		brakeLightOn = false;
	} 
	else if(gearSelected && ((potRequest <= 250) && (potRequest >= 0)) && (inverterStatus.motorSpeed > minimumRegenRpm)) // Regeneration torque requested and above minimum regen speed
	{
		// Note: negative torque values for regen!
		torqueRequest = map(potRequest, 0, 250, regenTorqueRequestLimit, 0);
		
		if(inverterStatus.motorSpeed < rampRegenRpm) // Ramp down regen torque for low motor speeds
		{
			int16_t speedLimitedTorqueRequest = map(inverterStatus.motorSpeed, minimumRegenRpm, rampRegenRpm, 0, regenTorqueRequestLimit);
			if(torqueRequest < speedLimitedTorqueRequest)
			{
				torqueRequest = speedLimitedTorqueRequest;
			}
		}
		
		if(torqueRequest < powerLimitedRegenTorque)
		{
			torqueRequest = powerLimitedRegenTorque;
		}
		
		if(torqueRequest < (brakeLightTorqueOn * 72))
		{
			brakeLightOn = true;
		}
		if(torqueRequest > (brakeLightTorqueOff * 72))
		{
			brakeLightOn = false;
		}
		
		int8_t expectedDirection = gear * inverterStatus.motorDirection; // Handle when selected gear does not match motor rotation direction
		
		torqueRequest *= expectedDirection;
	}	
	else 
	{
		torqueRequest = 0;
		brakeLightOn = false;
	}
	
	torqueRequest *= gear; // Sign of torque from selected gear
	
	digitalWrite(brakeLightPIN, brakeLightOn ? HIGH : LOW);
	
	return torqueRequest;
}


/**********************************
* Farenheit to celsius conversion *
**********************************/
int8_t fahrenheitToCelsius(uint8_t fahrenheit)
{
  //int8_t result = ((int16_t)fahrenheit - 32) * 5 / 9;
  int8_t result = ((int16_t)(fahrenheit - 32) * 9) >> 4;
  
  return result;
}


/*************************
* Nissan CRC calculation *
*************************/
static void nissanCRC(uint8_t *data)
{
	data[7] = 0;
	uint8_t crc = 0;
		
	for (int b=0; b<8; b++) {
		for (int i=7; i>=0; i--) {
			uint8_t bit = ((data[b] & (1 << i)) > 0) ? 1 : 0;
			if(crc >= 0x80) {
				crc = (byte)(((crc << 1) + bit) ^ 0x85);
			} else {
				crc = (byte)((crc << 1) + bit);
			}
		}
	}
	
	data[7] = crc;
}


/****************
* Init CAN-bus *
****************/
void initCAN()
{
	// CAN
	for(int i=0; i<20; i++)
	{
		if(CAN_OK == CAN.begin(MCP_STDEXT, CAN_250KBPS, MCP_8MHZ))    // Init car CAN bus : baudrate = 250k
		{
			CAN.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
					
			CAN.init_Mask(0,0,0x07FF0000);                // Init first mask
			CAN.init_Filt(0,0,0x01320000);                // Init first filter
			CAN.init_Filt(1,0,0x00910000);                // Init second filter
  
			CAN.init_Mask(1,0,0x07FF0000);                // Init second mask 
			CAN.init_Filt(2,0,0x01320000);                // Init third filter
			CAN.init_Filt(3,0,0x01000000);                // Init fouth filter
			CAN.init_Filt(4,0,0x01010000);                // Init fifth filter
			CAN.init_Filt(5,0,0x01020000);                // Init sixth filter
			
			Serial.println("CAN: MCP2515 init ok");
			break;
		}
		else
		{
			Serial.println("CAN: MCP2515 init failed");
		}
		delay(100);
	}
	
	// PTCAN   
	for(int i=0; i<20; i++)
	{
		if(CAN_OK == PTCAN.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ))    // Init car CAN bus : baudrate = 500k
		{
			PTCAN.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515 sends acks to received data.
					
			PTCAN.init_Mask(0,0,0x07FF0000);                // Init first mask
			PTCAN.init_Filt(0,0,0x01DA0000);                // Init first filter
			PTCAN.init_Filt(1,0,0x055A0000);                // Init second filter
  
			PTCAN.init_Mask(1,0,0x07FF0000);                // Init second mask 
			PTCAN.init_Filt(2,0,0x01DA0000);                // Init third filter
			PTCAN.init_Filt(3,0,0x055A0000);                // Init fouth filter
			PTCAN.init_Filt(4,0,0x055A0000);                // Init fifth filter
			PTCAN.init_Filt(5,0,0x055A0000);                // Init sixth filter
			
			Serial.println("PTCAN: MCP2515 init ok");
			break;
		}
		else
		{
			Serial.println("PTCAN: MCP2515 init failed");
		}
		delay(100);
	}
}