Helping adding code to an existing sketch

Status
Not open for further replies.

Dizzy

New member
Hi,

I'm running a Teensy 3.2 with CAN transceiver from pins 3(TX) and 4(RX). The below sketch is working fine in adding control for an electric motor on a 500k bus.

Code:
#include <Arduino.h>
#include <FlexCAN.h> //https://github.com/teachop/FlexCAN_Library 
#include <Smoothed.h> //https://github.com/MattFryer/Smoothed
#include <SPI.h>



/////////// Pin Assignments /////////
const int led = 13;


/////////// Variables //////////////
int rpm;
int mtemp;
int hstemp;
int amps;
int potnom;
int pot;
int run;
int dir = 0;
int brake;
int boost;
int maxBoost;
int pot2;
int fweak;
int minBoost;
int brkNomPedal;
int brkMax;
int baseRegen;
int maxRegen;
int idleThrot;
int slipstart;
int idleRPM;
int idleThrotMax;
int packVolt;
int neg = 4294967295;
float maxSlip;
float minSlip;
float fslip;
float fslipmin;
float ampMin;
float throtRamp;
bool startup;



CAN_message_t msg;
CAN_message_t inMsg;
CAN_filter_t filter;

//////////////Smoothing///////////////

Smoothed <int> idleRamp;
Smoothed <float> fslipRamp;


void setup() {

    pinMode(led, OUTPUT);
    Can0.begin(500000);

    //set filters for standard
    for (int i = 0; i < 8; i++)
    {
        Can0.setFilter(filter, i);
    }
    //set filters for extended
    for (int i = 9; i < 13; i++)
    {
        Can0.setFilter(filter, i);
    }

    digitalWrite(led, HIGH);
    Serial.begin(1152000);

    idleRamp.begin(SMOOTHED_AVERAGE, 60);
    fslipRamp.begin(SMOOTHED_AVERAGE, 60);

}

void loop() {

    while (Can0.available())
    {
        Can0.read(inMsg);
        decodeCAN();
    }

    parameterMap();
    boostMap();
    //idleThrottle();
    //regenStuff();
    

   
}




void decodeCAN() {

    if (inMsg.id == 0x135) {
        if ((((inMsg.buf[3] << 8) + inMsg.buf[2])) <= 2000) {

            amps = (((inMsg.buf[3] << 8) + inMsg.buf[2]) * 1.83);
        }
        else if ((((inMsg.buf[3] << 8) + inMsg.buf[2])) >= 3000) {

            amps = (((((inMsg.buf[3] << 8) + inMsg.buf[2]) - 65535) * 1.83) * -1);

        }
        rpm = (((inMsg.buf[1] << 8) + inMsg.buf[0]));

        if ((inMsg.buf[4]) > 0) {
            mtemp = (inMsg.buf[4]); //motor temp C
        }

        if ((inMsg.buf[5]) > 0) {
            hstemp = (inMsg.buf[5]); //heatsink temp C
        }
        if ((((inMsg.buf[7] << 8)) + inMsg.buf[6]) <= 2000) {
            potnom = (((inMsg.buf[7] << 8)) + inMsg.buf[6]);
        }
        else if ((((inMsg.buf[7] << 8)) + inMsg.buf[6]) >= 2000) {
            potnom = ((((inMsg.buf[7] << 8)) + inMsg.buf[6]) - 65535);
        }
    }

    else if (inMsg.id == 79) {

        dir = (inMsg.buf[0]);  
        brake = (inMsg.buf[1]); //din brake

    }

    else if (inMsg.id == 0x136) {

        run = (inMsg.buf[0]); //opmode
        packVolt = ((inMsg.buf[2] << 8) + inMsg.buf[1]); //UDC
    }

    else if (inMsg.id == 0x113) {
        pot = ((inMsg.buf[1] << 8) + inMsg.buf[0]); 
        pot2 = ((inMsg.buf[3] << 8) + inMsg.buf[2]);
    }


}



void parameterMap() {

    //boost
    maxBoost = 1720;
    minBoost = 1720;

    //fweak
    if (pot > 1800 && pot < 3200) {
        fweak = map(pot, 1800, 3200, 400, 258);
    }
    else if (pot >= 3200) {

        fweak = 258;
    }
    else {
        fweak = 400;
    }
    canSet(1, fweak);

    //fslipmin
    fslipmin = 2.3;
    canSet(4, fslipmin);

    //fslipmax
    maxSlip = (3.08 * 32);
    if (pot >= 2800) {
        minSlip = map(pot, 2800, 4095, (fslipmin * 32), maxSlip);

    }
    else { minSlip = (fslipmin * 32); }

    if (rpm <= 4500) {
        fslip = map(rpm, 0, 4200, minSlip, maxSlip);
    }
    else { fslip = maxSlip; }

    //fslipRamp.add(fslip);
    canSet(5, fslip / 32);


    // throtramp

    if (pot < 1500) {
        throtRamp = .45;
    }
    else if (pot >= 1500 && pot < 3000) {
        throtRamp = map(pot, 1500, 3700, 1, 25);
    }
    else {
        throtRamp = 25;

    }
    canSet(49, throtRamp);

    //slipstart
    slipstart = 32;
    canSet(52, slipstart);

    //ampmin
    ampMin = 1.1;
    canSet(51, ampMin);

}
void boostMap()  //sets boost lower for startup without OC trip, ramps higher with throttle for increased power
{
    if (pot > 3700) {
        boost = map(pot, 3700, 4095, minBoost, maxBoost);
        canSet(0, boost);
    }
    else {
        boost = minBoost;
        canSet(0, boost);
    }

}

void regenStuff() {

    // This method allows both pedal off regen in combination with variable pot2 value. Previously was not able to achieve this with recent firmware.

    baseRegen = 0; //base throttle off regen value
    maxRegen = 90;  //maximum full brake pressure regen value


    //brakenompedal
    /*if (rpm <= 2000) {
        brkNomPedal = map(rpm, 0, 2000, ((neg - (1 * 32)) / 32), ((neg - (baseRegen * 32))) / 32);  //reduces pedal regen with speed below 2,000 rpm.

    }  */

    if (pot2 > 3700) {
        brkNomPedal = ((neg - (maxRegen * 32)) / 32); //sets POT2 value for maximum regen
    }
    else {
        brkNomPedal = map(pot2, 600, 3700, ((neg - (baseRegen * 32)) / 32), ((neg - (maxRegen * 32))) / 32); //maps brake pedal regen between base and max
    }
    canSet(53, brkNomPedal);

    //brakemax
    brkMax = ((neg - (baseRegen * 32)) / 32);
    canSet(56, brkMax);
}


void idleThrottle() {

    if (run == 0) {
        canSet(64, 1);  // Sets idlemode to brakeoff when inverter is not in opmode "run"
    }
    else {
        canSet(64, 0); //Sets idlemode to alwayson when inverter is in "run"
    }

    idleRamp.add(pot2);
    idleThrot = map(idleRamp.get(), 600, 1020, idleThrotMax, 0);
    canSet(63, idleThrot);

    idleThrotMax = 15;

    idleRPM = 1750;
    canSet(62, idleRPM);
}

void canSet(int index, float value) {
    int val = (value * 32);  //scale value * 32 to make STM32 happy
    int byte1;
    int byte2;
    int byte3;
    int byte4;

    byte1 = val & 0xFF;  //bitshifting
    byte2 = (val >> 8) & 0xFF;
    byte3 = (val >> 16) & 0xFF;
    byte4 = (val >> 24) & 0xFF;



    msg.id = 0x601; //set parameter ID
    msg.len = 8;
    msg.buf[0] = 0x40;
    msg.buf[1] = 0x00;
    msg.buf[2] = 0x20;
    msg.buf[3] = index; //index value of parameter, boost = 0
    msg.buf[4] = byte1;
    msg.buf[5] = byte2;
    msg.buf[6] = byte3;
    msg.buf[7] = byte4;
    Can0.write(msg);
}



void debug() {

    Serial.print("Pot val:");
    Serial.println(pot);
    Serial.print("Fslip:");
    Serial.println(fslip/32);
    Serial.print("Fweak:");
    Serial.println(fweak);
    Serial.println("");
    Serial.println("");
    Serial.println("");
 
}



I wish to control another piece of equipment on the same 500k bus. I've found the below code which apparently works and I'd like to add it to my sketch above.



Code:
#include <Arduino.h>
#include <SPI.h>
#include <mcp_can.h>

const int SPI_CS_PIN = 9;
MCP_CAN CAN(SPI_CS_PIN);
int counter = 0;

const unsigned char voltage_03_8[2] = {0xA0, 0xAF};

void setup() {
Serial.begin(9600);

// Initialize CAN Bus Shield, set baudrate [/h1]
500k
while (CAN_OK != CAN.begin(CAN_500KBPS))
{
Serial.println("CAN BUS Shield init - FAILED.");
Serial.println("Retrying...");
delay(100);
}

Serial.println("CAN BUS Shield init - OK.");
}

void loop() {

// SENDING CAN MESSAGES - DC 2 DC converter
CAN.sendMsgBuf(0x1D4, 0, 2, voltage_03_8);

delay(100);

// RECEIVING CAN MESSAGES
if(counter == 10 && (CAN_MSGAVAIL == CAN.checkReceive())) {

unsigned char len = 0;
unsigned char buf[8];
unsigned long canId = 0;


CAN.readMsgBuf(&len, buf);
canId = CAN.getCanId();

if(canId == 0x1D6) {
double lv_output_voltage = (double)buf[2];
lv_output_voltage = lv_output_voltage / 12.7;

int temp1 = (int)buf[3];
temp1 = temp1 - 40;
int temp2 = (int)buf[4];
temp2 = temp2 - 40;

int lv_output_current = (int)buf[5];

Serial.print("ID; Serial.print(canId); Serial.print("] - ");
Serial.print("Volts Serial.print(lv_output_voltage); Serial.print(" : ");
Serial.print("Temp1 Serial.print(temp1); Serial.print(" : ");
Serial.print("Temp2 Serial.print(temp2); Serial.print(" : ");
Serial.print("Apms Serial.print(lv_output_current);

Serial.println();

}
counter = 0;

} // End if block - counter && can received.

counter++;
}

I'm a complete novice and willing to learn, but despite studying this for three days, I'm no further forward. Could some help me add this code into my main sketch?
 
Status
Not open for further replies.
Back
Top