Tatsuya Yamamoto / Mbed 2 deprecated UV_Robot_Nucleo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed X_NUCLEO_PLC01A1 ros_lib_melodic

uv_robot.cpp

Committer:
yamadola
Date:
2020-08-15
Revision:
2:72761505a690
Parent:
1:ef4b86795d79
Child:
3:ea5cfd721b53

File content as of revision 2:72761505a690:

#include "uv_robot.h"

UVRobot::UVRobot():_can(),_plc(),_rs485(),
_wheel(&_can, &_plc), _topLamp(2, 0x01, &_plc, &_rs485), _bottomLamp(3, &_plc), _footLamp(4, &_plc),
_enable(enablePin, &_plc), _error(errorSigPin, &_plc), _emgSW(emgSigPin, &_plc){}

enum state UVRobot::getState(){
    return _robotState;
}

void UVRobot::setState( enum state robotState ){
    _robotState = robotState;
}

void UVRobot::init(){
    setState( Init );
    _enable = 1;
    _error = 0;
    _wheel.init();
    _topLamp.init();
    _bottomLamp.init();
    _footLamp.init();
    setState( Idle );
}

void UVRobot::start(){
    _robotState = Run;
    std_msgs::Bool  sMsg;
    std_msgs::Float32MultiArray  rMsg;
    /* 
    * rMsg.data[0] : Right Wheel RPM
    * rMsg.data[1] : Left Wheel RPM
    * rMsg.data[2] : Top Lamp On/Off (On:1 Off:0)
    * rMsg.data[3] : Top Lamp Angle
    * rMsg.data[4] : Bottom Lamp On/Off (On:1 Off:0)
    * rMsg.data[5] : Foot Lamp On/Off (On:1 Off:0)
    * rMsg.data[6] : Error Signal (Stop:0 Run:1 Error:2 )
    */
    
    while(1){
        
        _RosI._nh.spinOnce();
        rMsg = _RosI.readMsg();
        
        _wheel.setVelocity( rMsg.data[0], rMsg.data[1] );
        _topLamp.setUVPower( (uint8_t)rMsg.data[2] );
        _topLamp.setLightAngle( rMsg.data[3] );
        _bottomLamp.setUVPower( (uint8_t)rMsg.data[4] );
        _footLamp.setUVPower( (uint8_t)rMsg.data[5] );
        
        _wheel.drive();
        _topLamp.drive();
        
        sMsg.data = _emgSW;
        _RosI.sendMsg(sMsg);
        wait_ms(5);
        
        if( _emgSW || rMsg.data[6]==2 ){
            UVRobot::error();
        }
    }
}

void UVRobot::stop(){
    std_msgs::Float32MultiArray  rMsg;
    
    _wheel.stop();
    _topLamp.stop();
    _bottomLamp.setUVPower(0);
    _footLamp.setUVPower(0);
    _robotState = Idle;
    
    while(1){
        _RosI._nh.spinOnce();
        rMsg = _RosI.readMsg();
        if(rMsg[6] == 1){
            UVRobot::start();
        }
    }
}

void UVRobot::error(){
    _enable = 0;
    _error = 1;
    _wheel.brake();
    _topLamp.stop();
    _bottomLamp.setUVPower(0);
    _footLamp.setUVPower(0);
    _robotState = Error;
}