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@1:ef4b86795d79, 2020-08-15 (annotated)

Committer:
yamadola
Date:
Sat Aug 15 09:09:33 2020 +0000
Revision:
1:ef4b86795d79
Parent:
0:43eb9ccc1583
Child:
2:72761505a690
whole program without debugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yamadola 0:43eb9ccc1583 1 #include "uv_robot.h"
yamadola 0:43eb9ccc1583 2
yamadola 1:ef4b86795d79 3 UVRobot::UVRobot():_can(),_plc(),_rs485(),
yamadola 1:ef4b86795d79 4 _wheel(&_can, &_plc), _topLamp(2, 0x01, &_plc, &_rs485), _bottomLamp(3, &_plc), _footLamp(4, &_plc),
yamadola 1:ef4b86795d79 5 _enable(enablePin, &_plc), _error(errorSigPin, &_plc), _emgSW(emgSigPin, &_plc){}
yamadola 1:ef4b86795d79 6
yamadola 0:43eb9ccc1583 7 enum state UVRobot::getState(){
yamadola 0:43eb9ccc1583 8 return _robotState;
yamadola 0:43eb9ccc1583 9 }
yamadola 0:43eb9ccc1583 10
yamadola 0:43eb9ccc1583 11 void UVRobot::setState( enum state robotState ){
yamadola 0:43eb9ccc1583 12 _robotState = robotState;
yamadola 0:43eb9ccc1583 13 }
yamadola 0:43eb9ccc1583 14
yamadola 0:43eb9ccc1583 15 void UVRobot::init(){
yamadola 0:43eb9ccc1583 16 setState( Init );
yamadola 1:ef4b86795d79 17 _enable = 1;
yamadola 1:ef4b86795d79 18 _error = 0;
yamadola 1:ef4b86795d79 19 _wheel.init();
yamadola 1:ef4b86795d79 20 _topLamp.init();
yamadola 1:ef4b86795d79 21 _bottomLamp.init();
yamadola 1:ef4b86795d79 22 _footLamp.init();
yamadola 0:43eb9ccc1583 23 setState( Idle );
yamadola 0:43eb9ccc1583 24 }
yamadola 0:43eb9ccc1583 25
yamadola 0:43eb9ccc1583 26 void UVRobot::start(){
yamadola 0:43eb9ccc1583 27 _robotState = Run;
yamadola 1:ef4b86795d79 28 std_msgs::Bool sMsg;
yamadola 1:ef4b86795d79 29 std_msgs::Float32MultiArray rMsg;
yamadola 1:ef4b86795d79 30 /*
yamadola 1:ef4b86795d79 31 * rMsg.data[0] : Right Wheel RPM
yamadola 1:ef4b86795d79 32 * rMsg.data[1] : Left Wheel RPM
yamadola 1:ef4b86795d79 33 * rMsg.data[2] : Top Lamp On/Off (On:1 Off:0)
yamadola 1:ef4b86795d79 34 * rMsg.data[3] : Top Lamp Angle
yamadola 1:ef4b86795d79 35 * rMsg.data[4] : Bottom Lamp On/Off (On:1 Off:0)
yamadola 1:ef4b86795d79 36 * rMsg.data[5] : Foot Lamp On/Off (On:1 Off:0)
yamadola 1:ef4b86795d79 37 * rMsg.data[6] : Error Signal (Error:1 Nominal:0)
yamadola 1:ef4b86795d79 38 */
yamadola 1:ef4b86795d79 39
yamadola 1:ef4b86795d79 40 while(1){
yamadola 1:ef4b86795d79 41 sMsg.data = _emgSW;
yamadola 1:ef4b86795d79 42 rMsg = _RosI.readMsg();
yamadola 1:ef4b86795d79 43
yamadola 1:ef4b86795d79 44 _wheel.setVelocity( rMsg.data[0], rMsg.data[1] );
yamadola 1:ef4b86795d79 45 _topLamp.setUVPower( (uint8_t)rMsg.data[2] );
yamadola 1:ef4b86795d79 46 _topLamp.setLightAngle( rMsg.data[3] );
yamadola 1:ef4b86795d79 47 _bottomLamp.setUVPower( (uint8_t)rMsg.data[4] );
yamadola 1:ef4b86795d79 48 _footLamp.setUVPower( (uint8_t)rMsg.data[5] );
yamadola 1:ef4b86795d79 49
yamadola 1:ef4b86795d79 50 _wheel.drive();
yamadola 1:ef4b86795d79 51 _topLamp.drive();
yamadola 1:ef4b86795d79 52
yamadola 1:ef4b86795d79 53 _RosI.sendMsg(sMsg);
yamadola 1:ef4b86795d79 54 _RosI._nh.spinOnce();
yamadola 1:ef4b86795d79 55 wait_ms(5);
yamadola 1:ef4b86795d79 56
yamadola 1:ef4b86795d79 57 if( _emgSW || (bool)rMsg.data[6] ){
yamadola 1:ef4b86795d79 58 UVRobot::error();
yamadola 1:ef4b86795d79 59 }
yamadola 1:ef4b86795d79 60 }
yamadola 0:43eb9ccc1583 61 }
yamadola 0:43eb9ccc1583 62
yamadola 0:43eb9ccc1583 63 void UVRobot::stop(){
yamadola 1:ef4b86795d79 64 _wheel.stop();
yamadola 1:ef4b86795d79 65 _topLamp.stop();
yamadola 1:ef4b86795d79 66 _bottomLamp.setUVPower(0);
yamadola 1:ef4b86795d79 67 _footLamp.setUVPower(0);
yamadola 0:43eb9ccc1583 68 _robotState = Idle;
yamadola 1:ef4b86795d79 69 }
yamadola 1:ef4b86795d79 70
yamadola 1:ef4b86795d79 71 void UVRobot::error(){
yamadola 1:ef4b86795d79 72 _wheel.brake();
yamadola 1:ef4b86795d79 73 _topLamp.stop();
yamadola 1:ef4b86795d79 74 _bottomLamp.setUVPower(0);
yamadola 1:ef4b86795d79 75 _footLamp.setUVPower(0);
yamadola 1:ef4b86795d79 76 _robotState = Error;
yamadola 0:43eb9ccc1583 77 }