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@3:ea5cfd721b53, 2020-08-30 (annotated)

Committer:
yamadola
Date:
Sun Aug 30 06:39:43 2020 +0000
Revision:
3:ea5cfd721b53
Parent:
2:72761505a690 
This code includes all functions for Amabie.; However, I have never debugged it with the real body.

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 3:ea5cfd721b53 17 _enable = 0;
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 3:ea5cfd721b53 30 bool enFlag = 0;
yamadola 1:ef4b86795d79 31 /*
yamadola 1:ef4b86795d79 32 * rMsg.data[0] : Right Wheel RPM
yamadola 1:ef4b86795d79 33 * rMsg.data[1] : Left Wheel RPM
yamadola 1:ef4b86795d79 34 * rMsg.data[2] : Top Lamp On/Off (On:1 Off:0)
yamadola 1:ef4b86795d79 35 * rMsg.data[3] : Top Lamp Angle
yamadola 1:ef4b86795d79 36 * rMsg.data[4] : Bottom Lamp On/Off (On:1 Off:0)
yamadola 1:ef4b86795d79 37 * rMsg.data[5] : Foot Lamp On/Off (On:1 Off:0)
yamadola 3:ea5cfd721b53 38 * rMsg.data[6] : State Signal (Stop:0 Run:1 Error:2 Reset:3)
yamadola 1:ef4b86795d79 39 */
yamadola 1:ef4b86795d79 40
yamadola 1:ef4b86795d79 41 while(1){
yamadola 2:72761505a690 42
yamadola 2:72761505a690 43 _RosI._nh.spinOnce();
yamadola 1:ef4b86795d79 44 rMsg = _RosI.readMsg();
yamadola 1:ef4b86795d79 45
yamadola 3:ea5cfd721b53 46 switch((int)rMsg.data[6]){
yamadola 3:ea5cfd721b53 47 case 0:
yamadola 3:ea5cfd721b53 48 UVRobot::stop();
yamadola 3:ea5cfd721b53 49 enFlag = 0;
yamadola 3:ea5cfd721b53 50 break;
yamadola 3:ea5cfd721b53 51
yamadola 3:ea5cfd721b53 52 case 1:
yamadola 3:ea5cfd721b53 53 if(enFlag == 0){ _enable = 1; enFlag = 1; wait_ms(500);}
yamadola 3:ea5cfd721b53 54 _wheel.setVelocity( rMsg.data[0], rMsg.data[1] );
yamadola 3:ea5cfd721b53 55 _topLamp.setUVPower( (uint8_t)rMsg.data[2] );
yamadola 3:ea5cfd721b53 56 _topLamp.setLightAngle( rMsg.data[3] );
yamadola 3:ea5cfd721b53 57 _bottomLamp.setUVPower( (uint8_t)rMsg.data[4] );
yamadola 3:ea5cfd721b53 58 _footLamp.setUVPower( (uint8_t)rMsg.data[5] );
yamadola 1:ef4b86795d79 59
yamadola 3:ea5cfd721b53 60 _wheel.drive();
yamadola 3:ea5cfd721b53 61 _topLamp.drive();
yamadola 3:ea5cfd721b53 62 break;
yamadola 3:ea5cfd721b53 63
yamadola 3:ea5cfd721b53 64 case 2:
yamadola 3:ea5cfd721b53 65 enFlag = 0;
yamadola 3:ea5cfd721b53 66 UVRobot::error();
yamadola 3:ea5cfd721b53 67 break;
yamadola 3:ea5cfd721b53 68
yamadola 3:ea5cfd721b53 69 default:
yamadola 3:ea5cfd721b53 70 enFlag = 0;
yamadola 3:ea5cfd721b53 71 UVRobot::stop();
yamadola 3:ea5cfd721b53 72 break;
yamadola 3:ea5cfd721b53 73 }
yamadola 3:ea5cfd721b53 74
yamadola 2:72761505a690 75 sMsg.data = _emgSW;
yamadola 1:ef4b86795d79 76 _RosI.sendMsg(sMsg);
yamadola 3:ea5cfd721b53 77 if( _emgSW || (_wheel.getBatteryVoltage() < 22.0) ){
yamadola 1:ef4b86795d79 78 UVRobot::error();
yamadola 1:ef4b86795d79 79 }
yamadola 3:ea5cfd721b53 80
yamadola 3:ea5cfd721b53 81 wait_ms(5);
yamadola 1:ef4b86795d79 82 }
yamadola 0:43eb9ccc1583 83 }
yamadola 0:43eb9ccc1583 84
yamadola 0:43eb9ccc1583 85 void UVRobot::stop(){
yamadola 3:ea5cfd721b53 86 _enable = 0;
yamadola 1:ef4b86795d79 87 _wheel.stop();
yamadola 1:ef4b86795d79 88 _topLamp.stop();
yamadola 1:ef4b86795d79 89 _bottomLamp.setUVPower(0);
yamadola 1:ef4b86795d79 90 _footLamp.setUVPower(0);
yamadola 1:ef4b86795d79 91 }
yamadola 1:ef4b86795d79 92
yamadola 1:ef4b86795d79 93 void UVRobot::error(){
yamadola 3:ea5cfd721b53 94 std_msgs::Float32MultiArray rMsg;
yamadola 3:ea5cfd721b53 95 while(1){
yamadola 3:ea5cfd721b53 96 _enable = 0;
yamadola 3:ea5cfd721b53 97 _error = 1;
yamadola 3:ea5cfd721b53 98 _wheel.brake();
yamadola 3:ea5cfd721b53 99 _topLamp.stop();
yamadola 3:ea5cfd721b53 100 _bottomLamp.setUVPower(0);
yamadola 3:ea5cfd721b53 101 _footLamp.setUVPower(0);
yamadola 3:ea5cfd721b53 102 _robotState = Error;
yamadola 3:ea5cfd721b53 103 _RosI._nh.spinOnce();
yamadola 3:ea5cfd721b53 104 rMsg = _RosI.readMsg();
yamadola 3:ea5cfd721b53 105 if(rMsg.data[6] == 3){ break; }
yamadola 3:ea5cfd721b53 106 }
yamadola 0:43eb9ccc1583 107 }