#include <SPI.h>
#include <mcp2515.h>

struct can_frame canMsg1; // variabele voor het can bericht 1.
struct can_frame canMsg2; // variabele voor het can bericht 2.

MCP2515 mcp2515(10);


float HuidigeSnelheid = 0; // float kan decimalen bevatten.
float GewensteSnelheid = 0;
float Responsiveness = 0.1; // Hoe snel de snelheid aangepast wordt.
float TimeStep = 0.1; // Tijdstap in seconden (100 ms = 0.1)


bool StatusPin3 = false; 





void setup() {

Serial.begin(9600);
SPI.begin(); // begint de seriële communicatie.

mcp2515.reset();
mcp2515.setBitrate(CAN_250KBPS, MCP_8MHZ);
mcp2515.setNormalMode();


pinMode(3,INPUT); // zijpikkel ingang.

}



void loop() {

if (mcp2515.readMessage(&canMsg1)== MCP2515::ERROR_OK && canMsg1.can_id == 0xBA){

byte ThrottleWaarde = canMsg1.data[0]; // throttlewaarde binnenlezen.


GewensteSnelheid = map(ThrottleWaarde, 0,100,0,70); // LIN_TRAFO van throttlewaarde tot max 70 km/u.
HuidigeSnelheid = HuidigeSnelheid + (GewensteSnelheid - HuidigeSnelheid) * Responsiveness * TimeStep;

if (HuidigeSnelheid < 0) HuidigeSnelheid = 0;
if (HuidigeSnelheid > 70) HuidigeSnelheid = 70;

}

StatusPin3 = digitalRead(3); // Status van de zijpikkel.


canMsg2.can_id = 0xB9;
canMsg2.can_dlc = 8;
canMsg2.data[0] = (byte) round (HuidigeSnelheid); // huidige snelheid (float --> decimale waarde) afronden naar een volledig cijfer.
canMsg2.data[1] = StatusPin3; // zijpikkel status.
canMsg2.data[2] = 0x00;
canMsg2.data[3] = 0x00;
canMsg2.data[4] = 0x00;
canMsg2.data[5] = 0x00;
canMsg2.data[6] = 0x00;
canMsg2.data[7] = 0x00;

mcp2515.sendMessage(& canMsg2);


delay (100);
}