code.cpp

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <unistd.h>

#include <chrono>

#include <iomanip>

#include <iostream>

#include <string>

#include "can_wrap.hpp"

using can::operator<<;
#include "candata.h"
void wspeed_frame( const can_frame frame )

{

    candata_vcu_wheel_speeds_t data;

    candata_vcu_wheel_speeds_unpack(&data, frame.data, frame.can_dlc);




    if(!candata_vcu_wheel_speeds_fl_wheel_speed_is_in_range(data.fl_wheel_speed))

    throw std::out_of_range("fl_wheel");




    if(!candata_vcu_wheel_speeds_fr_wheel_speed_is_in_range(data.fr_wheel_speed))

    throw std::out_of_range("fr_wheel");




    if(!candata_vcu_wheel_speeds_rl_wheel_speed_is_in_range(data.rl_wheel_speed))

    throw std::out_of_range("rl_wheel");




    if(!candata_vcu_wheel_speeds_rr_wheel_speed_is_in_range(data.rr_wheel_speed))

    throw std::out_of_range("rr_wheel");




    const int fl_wheel = candata_vcu_wheel_speeds_fl_wheel_speed_decode(data.fl_wheel_speed);

    const int fr_wheel = candata_vcu_wheel_speeds_fr_wheel_speed_decode(data.fr_wheel_speed);

    const int rl_wheel = candata_vcu_wheel_speeds_rl_wheel_speed_decode(data.rl_wheel_speed);

    const int rr_wheel = candata_vcu_wheel_speeds_rr_wheel_speed_decode(data.rr_wheel_speed);




}





void torque_frame(const can_frame frame)

{

    candata_ai_drive_request_t data;

    candata_ai_drive_request_unpack(&data, frame.data, frame.can_dlc);




    if(!candata_ai_drive_request_front_trq_request_is_in_range(data.front_trq_request))

    throw std::out_of_range("front_trq");




    if(!candata_ai_drive_request_steering_request_is_in_range(data.steering_request))

    throw std::out_of_range("steering");




    if(!candata_ai_drive_request_rear_trq_request_is_in_range(data.rear_trq_request))

    throw std::out_of_range("rear_trq");




    const int front_trq = candata_ai_drive_request_front_trq_request_decode(data.front_trq_request);

    const int steering = candata_ai_drive_request_steering_request_decode(data.steering_request);

    const int rear_trq = candata_ai_drive_request_rear_trq_request_decode(data.rear_trq_request);

}




void vlt_frame( const can_frame frame )

{

    candata_vcu_battery_t data;

    candata_vcu_battery_unpack(&data, frame.data, frame.can_dlc);




    if(!candata_vcu_battery_voltage_is_in_range(data.voltage))

    throw std::out_of_range("voltage");




    const float vol = candata_vcu_battery_voltage_decode(data.voltage);

    return vol;

}




void fwspeed_frame( const can_frame frame )

{

    candata_vcu_wheel_speeds_t data;

    candata_vcu_wheel_speeds_unpack(&data, frame.data, frame.can_dlc);




    if(!candata_vcu_wheel_speeds_fl_wheel_speed_is_in_range(data.fl_wheel_speed))

    throw std::out_of_range("fl_wheel");




    if(!candata_vcu_wheel_speeds_fr_wheel_speed_is_in_range(data.fr_wheel_speed))

    throw std::out_of_range("fr_wheel");




    const int fl_wheel = candata_vcu_wheel_speeds_fl_wheel_speed_decode(data.fl_wheel_speed);

    const int fr_wheel = candata_vcu_wheel_speeds_fr_wheel_speed_decode(data.fr_wheel_speed);




    if(fl_wheel < fr_wheel)

    {

        return fr_wheel;

    }

    else if(fl_wheel > fr_wheel){

        return fl_wheel;

    }

    else{

        return fr_wheel;

    }

 

}




void rwspeed_frame( const can_frame frame )

{

    candata_vcu_wheel_speeds_t data;

    candata_vcu_wheel_speeds_unpack(&data, frame.data, frame.can_dlc);




    if(!candata_vcu_wheel_speeds_rl_wheel_speed_is_in_range(data.rl_wheel_speed))

    throw std::out_of_range("rl_wheel");




    if(!candata_vcu_wheel_speeds_rr_wheel_speed_is_in_range(data.rr_wheel_speed))

    throw std::out_of_range("rr_wheel");




    const int rl_wheel = candata_vcu_wheel_speeds_rl_wheel_speed_decode(data.rl_wheel_speed);

    const int rr_wheel = candata_vcu_wheel_speeds_rr_wheel_speed_decode(data.rr_wheel_speed);




    if(rl_wheel < rr_wheel)

    {

        return rr_wheel;

    }

    else if(rl_wheel > rr_wheel){

        return rl_wheel;

    }

    else{

        return rr_wheel;

    }

}




int tqrpm(int p){

    int y;

    if(p>700)

    {

        y=50;

    }

    else if((p>600) && (p<700))

    {

        y=85;

    }

    else if((p>500) && (p<600))

    {

        y=100;

    }

    else if((p>400) && (p<500))

    {

        y=120;

    }

    else if((p>300) && (p<400))

    {

        y=150;

    }

    return y;

}




void crt_frame( const can_frame frame )

{

    const float b=0.6721;

    const float q=0.879;




    candata_motor_current_t data;

   

    const double fct = candata_motor_current_front_current_decode(data.front_current);

    const double rct = candata_motor_current_rear_current_decode(data.rear_current);




    int vt = vlt_frame();

    if(vt<=2.8){

        int tq =20;

        int rfw = fwspeed_frame();

        int rrw = rwspeed_frame();

        fct = (tq/vt)*(1+(q*q*(tq/100))+(rfw*(b/100)*tq));

        rct = (tq/vt)*(1+(q*q*(tq/100))+(rrw*(b/100)*tq));




        if(!candata_motor_current_front_current_is_in_range(data.fc))

        throw std::out_of_range("front current");




        (data.front_current) = candata_motor_current_front_current_encode(data.fc);

       

        if(!candata_motor_current_rear_current_is_in_range(data.rc))

        throw std::out_of_range("Rear current");




        (data.rear_current) = candata_motor_current_rear_current_encode(data.rc);




        candata_motor_current_pack(&data, frame.data, frame.can_dlc)

    }

    else if(vt>2.8){




        int rfw = fwspeed_frame();

        int tfw = tqrpm(rfw);

        int rrw = rwheelspeed_frame();

        int trw = tqrpm(rrw);




        fct = (tfw/vt)*(1+(q*q*(tfw/100))+(rfw*(b/100)*tfw));

        rct = (trw/vt)*(1+(q*q*(trw/100))+(rrw*(b/100)*trw));




        if(!candata_motor_current_front_current_is_in_range(data.fc))

        throw std::out_of_range("front current");




        (data.front_current) = candata_motor_current_front_current_encode(data.fc);

       

        if(!candata_motor_current_rear_current_is_in_range(data.rc))

        throw std::out_of_range("Rear current");




        (data.rear_current) = candata_motor_current_rear_current_encode(data.rc);




        candata_motor_current_pack(&data, frame.data, frame.can_dlc)

    }




}




void pr_frame( const can_frame frame )

{

    switch( frame.can_id )

    {

        case : CANDATA_AI_DRIVE_REQUEST_FRAME_ID

            torque_frame( frame );

            break;

        case : CANDATA_VCU_WHEEL_SPEEDS_FRAME_ID

            wspeed_frame( frame );

            break;

        case : CANDATA_VCU_BATTERY_FRAME_ID

            vlt_frame( frame );

            break;

        default:

            /* unknown frame */

            break;

    }

}




int main( int argc, char* argv[] )

{

    const std::string canChannel = "vcan0";




    try

    {

        const int canSocket = can::connect( canChannel );




        for( int c=0; c<100; ++c )

        {

            const can_frame frame = can::read( canSocket );

            pr_frame( frame );

        }




        can::close( canSocket );

    }

    catch( const std::exception& e )

    {

        std::cerr << e.what() << std::endl;

    }

    {

        const std::string canChannel = "vcan0";

        const int canSocket = can::connect( canChannel );




        for( int c=0; c<100; ++c )

        {

            can_frame frame;

            std::memset( &frame, 0, sizeof(frame) );




            frame.can_id = CANDATA_MOTOR_CURRENT_FRAME_ID;

            frame.can_dlc = 4;

            crt_frame(frame);

       

            try

            {

                std::cout << frame << std::endl;

                can::write( canSocket, frame );

            }

            catch( const std::runtime_error& e )

            {

                std::cerr << e.what() << std::endl;

            }




            std::this_thread::sleep_for( std::chrono::seconds( 1 ) );

           

        }

       can::close( canSocket );

       

    }

   

    return 0;

}