tarou yamada / YMotorDriverServo

YMotorDriverServo.cpp@2:16e29a3a8f58, 2015-10-14 (annotated)

Committer:
inst
Date:
Wed Oct 14 06:04:45 2015 +0000
Revision:
2:16e29a3a8f58
Parent:
1:d1ca02f9536c 

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


inst

0:9931e2ddc451


     1

#include "mbed.h"





inst

0:9931e2ddc451


     2

#include "YMotorDriverServo.h"





inst

0:9931e2ddc451


     3

#include "YMotorDriverBase.h"





inst

0:9931e2ddc451


     4







inst

2:16e29a3a8f58


     5

// １ループあたりだいたい80us程度





inst

2:16e29a3a8f58


     6







inst

2:16e29a3a8f58


     7

const float YMotorDriverServo::mAllowableError[] = {





inst

2:16e29a3a8f58


     8

    0.030,  // Steering





inst

2:16e29a3a8f58


     9

    0.030,  // Steering





inst

2:16e29a3a8f58


    10

    0.030,  // Steering





inst

2:16e29a3a8f58


    11

    0.010,  // Position Manager





inst

2:16e29a3a8f58


    12

    0.012   // Angle Manager





inst

2:16e29a3a8f58


    13

};





inst

2:16e29a3a8f58


    14

const PinName YMotorDriverServo::mAnalogInPinName   = dp10;





inst

2:16e29a3a8f58


    15

const float YMotorDriverServo::mLowPassFilterCoeff[] = {





inst

2:16e29a3a8f58


    16

    0.10f,   // Steering





inst

2:16e29a3a8f58


    17

    0.10f,   // Steering





inst

2:16e29a3a8f58


    18

    0.10f,   // Steering





inst

2:16e29a3a8f58


    19

    0.865f,  // PositionManager





inst

2:16e29a3a8f58


    20

    0.80f    // Angle Manager





inst

2:16e29a3a8f58


    21

};





inst

2:16e29a3a8f58


    22







inst

0:9931e2ddc451


    23

const float YMotorDriverServo::mPCoeff[] = {





inst

2:16e29a3a8f58


    24

    3.0f,   // Steering





inst

2:16e29a3a8f58


    25

    3.0f,   // Steering





inst

2:16e29a3a8f58


    26

    3.0f,   // Steering





inst

2:16e29a3a8f58


    27

    3.0f,   // Position Manager





inst

2:16e29a3a8f58


    28

    3.0f    // Angle Manager





inst

0:9931e2ddc451


    29

};





inst

0:9931e2ddc451


    30

const float YMotorDriverServo::mICoeff[] = {





inst

2:16e29a3a8f58


    31

    0.020f,     // Steering





inst

2:16e29a3a8f58


    32

    0.020f,     // Steering





inst

2:16e29a3a8f58


    33

    0.020f,     // Steering





inst

2:16e29a3a8f58


    34

    0.000f,     // Position Manager





inst

2:16e29a3a8f58


    35

    0.000f      // Angle Manager





inst

0:9931e2ddc451


    36

};





inst

0:9931e2ddc451


    37

const float YMotorDriverServo::mDCoeff[] = {





inst

2:16e29a3a8f58


    38

    0.020f,     // Steering





inst

2:16e29a3a8f58


    39

    0.020f,     // Steering





inst

2:16e29a3a8f58


    40

    0.020f,     // Steering





inst

2:16e29a3a8f58


    41

    0.000f,     // Position Manager





inst

2:16e29a3a8f58


    42

    0.000f      // Angle Manager





inst

2:16e29a3a8f58


    43

};





inst

2:16e29a3a8f58


    44

const float YMotorDriverServo::mServoMaxDuty[] = {





inst

2:16e29a3a8f58


    45

    0.80f,     // Steering





inst

2:16e29a3a8f58


    46

    0.80f,     // Steering





inst

2:16e29a3a8f58


    47

    0.80f,     // Steering





inst

2:16e29a3a8f58


    48

    0.80f,     // Position Manager





inst

2:16e29a3a8f58


    49

    0.90f      // Angle Manager





inst

2:16e29a3a8f58


    50

};





inst

2:16e29a3a8f58


    51

const float YMotorDriverServo::mServoMinDuty[] = {





inst

2:16e29a3a8f58


    52

    0.43f,     // Steering





inst

2:16e29a3a8f58


    53

    0.43f,     // Steering





inst

2:16e29a3a8f58


    54

    0.43f,     // Steering





inst

2:16e29a3a8f58


    55

    0.40f,     // Position Manager





inst

2:16e29a3a8f58


    56

    0.85f      // Angle Manager





inst

0:9931e2ddc451


    57

};





inst

0:9931e2ddc451


    58







inst

2:16e29a3a8f58


    59

const float YMotorDriverServo::mServoRangeMin[] = {





inst

2:16e29a3a8f58


    60

    // Steering





inst

2:16e29a3a8f58


    61

    0.145098f,





inst

2:16e29a3a8f58


    62

    0.078431f,





inst

2:16e29a3a8f58


    63

    0.168627f,





inst

2:16e29a3a8f58


    64

    // Position Manager





inst

2:16e29a3a8f58


    65

    //0.0f,





inst

2:16e29a3a8f58


    66

    0.262745f,





inst

2:16e29a3a8f58


    67

    //0.196078f,





inst

2:16e29a3a8f58


    68

    // AngleManager





inst

2:16e29a3a8f58


    69

    0.00f





inst

2:16e29a3a8f58


    70

};





inst

1:d1ca02f9536c


    71







inst

2:16e29a3a8f58


    72

const float YMotorDriverServo::mServoRangeMax[] = {





inst

2:16e29a3a8f58


    73

    // Steering





inst

2:16e29a3a8f58


    74

    0.800000,





inst

2:16e29a3a8f58


    75

    0.768627f,





inst

2:16e29a3a8f58


    76

    0.850980f,





inst

2:16e29a3a8f58


    77

    // Position Manager





inst

2:16e29a3a8f58


    78

    //0.901961f,





inst

2:16e29a3a8f58


    79

    0.937255f,





inst

2:16e29a3a8f58


    80

    // AngleManager





inst

2:16e29a3a8f58


    81

    1.00f





inst

2:16e29a3a8f58


    82

};





inst

2:16e29a3a8f58


    83







inst

2:16e29a3a8f58


    84

YMotorDriverServo::YMotorDriverServo( char address, int id ) :





inst

2:16e29a3a8f58


    85

        YMotorDriverBase( address, mServoMaxDuty[ id ], mServoMinDuty[ id ] ),





inst

2:16e29a3a8f58


    86

        PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){





inst

2:16e29a3a8f58


    87

            





inst

1:d1ca02f9536c


    88

    mAnalogIn   = new AnalogIn( mAnalogInPinName );





inst

1:d1ca02f9536c


    89

    mPosition   = 0.5f;





inst

0:9931e2ddc451


    90

    mTargetPosition = 0.5f;





inst

1:d1ca02f9536c


    91

    mHasWorked  = false;





inst

1:d1ca02f9536c


    92

    mID         = id;





inst

0:9931e2ddc451


    93

}





inst

0:9931e2ddc451


    94







inst

0:9931e2ddc451


    95

YMotorDriverServo::~YMotorDriverServo(){





inst

0:9931e2ddc451


    96

    delete mAnalogIn;





inst

0:9931e2ddc451


    97

}





inst

0:9931e2ddc451


    98







inst

1:d1ca02f9536c


    99

void YMotorDriverServo::updateSpecial(){





inst

0:9931e2ddc451


   100

     // low pass filter





inst

2:16e29a3a8f58


   101

     mPosition = mAnalogIn->read() * ( 1.0f - mLowPassFilterCoeff[ mID ] ) + mPosition * mLowPassFilterCoeff[ mID ];





inst

2:16e29a3a8f58


   102

     





inst

2:16e29a3a8f58


   103

     updateServoSpecial();





inst

2:16e29a3a8f58


   104

     





inst

2:16e29a3a8f58


   105

     float diff = mTargetPosition - mPosition;





inst

2:16e29a3a8f58


   106

     mHasWorked = ( abs( diff ) < mAllowableError[ mID ] );





inst

2:16e29a3a8f58


   107

     





inst

2:16e29a3a8f58


   108

     updatePID( diff );





inst

0:9931e2ddc451


   109

     





inst

1:d1ca02f9536c


   110

     // 目標角度に達したらLEDを点灯する





inst

2:16e29a3a8f58


   111

    mLED->write( mHasWorked );





inst

2:16e29a3a8f58


   112

}





inst

2:16e29a3a8f58


   113







inst

2:16e29a3a8f58


   114

float map( float val, float oldMin, float oldMax, float newMin, float newMax ){





inst

2:16e29a3a8f58


   115

    return ( newMax - newMin ) * ( val - oldMin ) / ( oldMax - oldMin ) + newMin;





inst

1:d1ca02f9536c


   116

}





inst

1:d1ca02f9536c


   117







inst

1:d1ca02f9536c


   118

void YMotorDriverServo::updateI2CSlave(){





inst

1:d1ca02f9536c


   119

    switch ( mI2C->receive() ){





inst

0:9931e2ddc451


   120

        case I2CSlave::ReadAddressed:{





inst

0:9931e2ddc451


   121

            char buf[] = {





inst

2:16e29a3a8f58


   122

                // 0Byte目は、もう目的の角度に達したなら1、違うなら0





inst

2:16e29a3a8f58


   123

                mHasWorked,





inst

2:16e29a3a8f58


   124

                // 1Byte目は現在の位置0-255





inst

2:16e29a3a8f58


   125

                static_cast< char >( map( mPosition, mServoRangeMin[ mID ], mServoRangeMax[ mID ], 0.0f, 1.0f ) * 255.0f )





inst

0:9931e2ddc451


   126

            };





inst

0:9931e2ddc451


   127

            mI2C->write( buf, 2 );





inst

0:9931e2ddc451


   128

            break;





inst

0:9931e2ddc451


   129

        }





inst

0:9931e2ddc451


   130

        case I2CSlave::WriteGeneral:





inst

0:9931e2ddc451


   131

            break;





inst

1:d1ca02f9536c


   132

        





inst

0:9931e2ddc451


   133

        case I2CSlave::WriteAddressed:{





inst

0:9931e2ddc451


   134

            char buf;





inst

0:9931e2ddc451


   135

            mI2C->read( &buf, 1 );





inst

0:9931e2ddc451


   136

            // 0~255で移動させる位置を指定する





inst

2:16e29a3a8f58


   137

            /*





inst

2:16e29a3a8f58


   138

            float p = static_cast< float >( buf ) / 255.0f;





inst

2:16e29a3a8f58


   139

            setTargetPosition( map( p, 0.0f, 1.0f, mServoRangeMin[ mID ], mServoRangeMax[ mID ] ) );





inst

2:16e29a3a8f58


   140

            */





inst

2:16e29a3a8f58


   141

            setTargetPosition( static_cast< float >( buf ) / 255.0f );





inst

0:9931e2ddc451


   142

            break;





inst

0:9931e2ddc451


   143

        }





inst

1:d1ca02f9536c


   144

        





inst

0:9931e2ddc451


   145

        case I2CSlave::NoData:





inst

0:9931e2ddc451


   146

            break;





inst

0:9931e2ddc451


   147

    }





inst

0:9931e2ddc451


   148

}





inst

0:9931e2ddc451


   149







inst

0:9931e2ddc451


   150

void YMotorDriverServo::control( float c ){





inst

2:16e29a3a8f58


   151

     if ( mHasWorked ){





inst

0:9931e2ddc451


   152

        setMotorAction( BRAKE );





inst

0:9931e2ddc451


   153

        return;





inst

0:9931e2ddc451


   154

    }





inst

0:9931e2ddc451


   155

     





inst

0:9931e2ddc451


   156

     if ( c > 0.0f ){





inst

0:9931e2ddc451


   157

        setMotorAction( FORWARD );





inst

0:9931e2ddc451


   158

    } else if ( c < 0.0f ){





inst

0:9931e2ddc451


   159

        setMotorAction( REVERSE );





inst

0:9931e2ddc451


   160

    }





inst

0:9931e2ddc451


   161

    





inst

2:16e29a3a8f58


   162

    setPercent( abs( c ) );





inst

0:9931e2ddc451


   163

}