Diff: uv_robot.cpp

Revision:

3:ea5cfd721b53

Parent:

2:72761505a690

--- a/uv_robot.cpp	Sat Aug 15 09:21:36 2020 +0000
+++ b/uv_robot.cpp	Sun Aug 30 06:39:43 2020 +0000
@@ -14,7 +14,7 @@
 
 void UVRobot::init(){
     setState( Init );
-    _enable = 1;
+    _enable = 0;
     _error = 0;
     _wheel.init();
     _topLamp.init();
@@ -27,6 +27,7 @@
     _robotState = Run;
     std_msgs::Bool  sMsg;
     std_msgs::Float32MultiArray  rMsg;
+    bool enFlag = 0;
     /* 
     * rMsg.data[0] : Right Wheel RPM
     * rMsg.data[1] : Left Wheel RPM
@@ -34,7 +35,7 @@
     * 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 )
+    * rMsg.data[6] : State Signal (Stop:0 Run:1 Error:2 Reset:3)
     */
     
     while(1){
@@ -42,49 +43,65 @@
         _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] );
+        switch((int)rMsg.data[6]){
+            case 0:
+                UVRobot::stop();
+                enFlag = 0;
+                break;
+            
+            case 1:
+                if(enFlag == 0){ _enable = 1; enFlag = 1; wait_ms(500);}
+                _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();
-        
+                _wheel.drive();
+                _topLamp.drive();
+                break;
+            
+            case 2:
+                enFlag = 0;
+                UVRobot::error();
+                break;
+            
+            default:
+                enFlag = 0;
+                UVRobot::stop();
+                break;                
+        }
+                
         sMsg.data = _emgSW;
         _RosI.sendMsg(sMsg);
-        wait_ms(5);
-        
-        if( _emgSW || rMsg.data[6]==2 ){
+        if( _emgSW || (_wheel.getBatteryVoltage() < 22.0) ){
             UVRobot::error();
         }
+        
+        wait_ms(5);
     }
 }
 
 void UVRobot::stop(){
-    std_msgs::Float32MultiArray  rMsg;
-    
+    _enable = 0;
     _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;
+    std_msgs::Float32MultiArray  rMsg;
+    while(1){
+        _enable = 0;
+        _error = 1;
+        _wheel.brake();
+        _topLamp.stop();
+        _bottomLamp.setUVPower(0);
+        _footLamp.setUVPower(0);
+        _robotState = Error;
+        _RosI._nh.spinOnce();
+        rMsg = _RosI.readMsg();
+        if(rMsg.data[6] == 3){ break; }
+    }
 }
\ No newline at end of file