Revision 3:ea5cfd721b53, committed 2020-08-30

Comitter:

yamadola

Date:

Sun Aug 30 06:39:43 2020 +0000

Parent:

2:72761505a690

Commit message:

This code includes all functions for Amabie.; However, I have never debugged it with the real body.

Changed in this revision

--- a/can_controller.cpp	Sat Aug 15 09:21:36 2020 +0000
+++ b/can_controller.cpp	Sun Aug 30 06:39:43 2020 +0000
@@ -4,7 +4,16 @@
  * @brief  Initialize CAN controller
  * @param  None
 */
-CANController::CANController():can1(MBED_CONF_APP_CAN1_RD, MBED_CONF_APP_CAN1_TD){}
+CANController::CANController():_can1(MBED_CONF_APP_CAN1_RD, MBED_CONF_APP_CAN1_TD){
+//    _NoData[0] = 'N';
+//    _NoData[1] = 'o';
+//    _NoData[2] = ',';
+//    _NoData[3] = 'D';
+//    _NoData[4] = 'a';
+//    _NoData[5] = 'T';
+//    _NoData[6] = 'a';
+//    _NoData[7] = '.';
+}
 
 /**
  * @brief  Send CAN message
@@ -14,10 +23,19 @@
 */
 int CANController::sendMessage( uint16_t ID, char *msgData ){
     CANMessage msg(ID, msgData, 8, CANData, CANStandard);
-    int successFlag = can1.write(msg);
+    int successFlag = _can1.write(msg);
     if(successFlag){
         return 0;
     }else{
         return -1;
     }
 }
+
+unsigned char *CANController::recieveMessage( uint16_t ID ){
+    while(_can1.read(_rmsg)){
+        if(_rmsg.id == ID){
+            return _rmsg.data;
+        }
+    }
+    return _NoData;
+}
\ No newline at end of file

--- a/can_controller.h	Sat Aug 15 09:21:36 2020 +0000
+++ b/can_controller.h	Sun Aug 30 06:39:43 2020 +0000
@@ -5,11 +5,14 @@
 
 class CANController{
 private:
-    CAN can1;
+    CAN _can1;
+    CANMessage _rmsg;
 public:
     CANController();
     int sendMessage( uint16_t ID, char *msgData );
-    void recieveMessage();
+    unsigned char* recieveMessage( uint16_t ID );
+    
+    unsigned char *_NoData;
 };
 
 #endif
\ No newline at end of file

--- a/main.cpp	Sat Aug 15 09:21:36 2020 +0000
+++ b/main.cpp	Sun Aug 30 06:39:43 2020 +0000
@@ -10,4 +10,5 @@
     while (1) {
         
     }
+    return 0;
 }
\ No newline at end of file

--- a/ms-94bzb.cpp	Sat Aug 15 09:21:36 2020 +0000
+++ b/ms-94bzb.cpp	Sun Aug 30 06:39:43 2020 +0000
@@ -84,4 +84,14 @@
 */
 void MS_94BZB::setAcceleration(uint16_t accel){
     _accel = (uint16_t)(_gearRatio*accel);
+}
+
+uint16_t MS_94BZB::getInputVoltage(){
+    uint16_t CAN_ID = STATE_COMMAND_2|_motorID;
+    unsigned char *rMsg;
+    rMsg = _pCanController->recieveMessage( CAN_ID );
+    if(rMsg != _pCanController->_NoData){
+        _voltage = rMsg[5]<<8 + rMsg[6];
+    }
+    return _voltage;
 }
\ No newline at end of file

--- a/ms-94bzb.h	Sat Aug 15 09:21:36 2020 +0000
+++ b/ms-94bzb.h	Sun Aug 30 06:39:43 2020 +0000
@@ -18,6 +18,8 @@
     static const uint16_t CTL_COMMAND = (0x09<<7);
     static const uint8_t PWM_CTL = 0;
     static const uint8_t CAN_CTL = 1;
+    
+    static const uint16_t STATE_COMMAND_2 = (0x07<<7);
 
     /* Pointer of CAN controller */
     CANController *_pCanController;
@@ -34,6 +36,8 @@
     /* Turning acceleration of brush-less motor (100-11,164[rpm/s])*/
     uint16_t _accel;
     
+    uint16_t _voltage;
+    
 public:
     MS_94BZB( uint8_t motorID, CANController *pCanController);
     virtual void motorInit();
@@ -42,6 +46,7 @@
     virtual void free();
     void setAngularVelocity(float avel);
     void setAcceleration(uint16_t accel);
+    uint16_t getInputVoltage();
 };
 
 #endif
\ No newline at end of file

--- 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

--- a/wheel.cpp	Sat Aug 15 09:21:36 2020 +0000
+++ b/wheel.cpp	Sun Aug 30 06:39:43 2020 +0000
@@ -67,4 +67,8 @@
     wait(0.1);
     _brake = 1;
     _brakeState = 1;
+}
+
+float Wheel::getBatteryVoltage(){
+    return _leftWheel.getInputVoltage()*0.1;
 }
\ No newline at end of file

--- a/wheel.h	Sat Aug 15 09:21:36 2020 +0000
+++ b/wheel.h	Sun Aug 30 06:39:43 2020 +0000
@@ -23,6 +23,7 @@
     void stop();
     void setVelocity( float lVelocity, float rVelocity );
     void brake();
+    float getBatteryVoltage();
 };
 
 #endif
\ No newline at end of file