Andrew's git - arduino-weather.git/blob - libraries/Mitov/Mitov_Sabertooth

   1////////////////////////////////////////////////////////////////////////////////
   2//                                                                            //
   3//     This software is supplied under the terms of a license agreement or    //
   4//     nondisclosure agreement with Mitov Software and may not be copied      //
   5//     or disclosed except in accordance with the terms of that agreement.    //
   6//         Copyright(c) 2002-2016 Mitov Software. All Rights Reserved.        //
   7//                                                                            //
   8////////////////////////////////////////////////////////////////////////////////
   9
  10#ifndef _MITOV_SABERTOOTH_DUALMOTOR_h
  11#define _MITOV_SABERTOOTH_DUALMOTOR_h
  12
  13#include <Mitov.h>
  14
  15namespace Mitov
  16{
  17        class SabertoothDualMotorBasic : public Mitov::EnabledComponent
  18        {
  19                typedef Mitov::EnabledComponent inherited;
  20
  21        public:
  22                OpenWire::SinkPin       MotorsInputPins[ 2 ];
  23
  24        public:
  25                void SetEnabled( bool AValue )
  26                {
  27                        if( AValue == Enabled )
  28                                return;
  29
  30                        Enabled = AValue;
  31                        if( Enabled )
  32                                SendStart();
  33
  34                        else
  35                                SendStop();
  36                }
  37
  38        protected:
  39                Mitov::BasicSerialPort &FSerial;
  40
  41        protected:
  42                float FSpeeds[2] = { -1.0, -1.0 };
  43
  44        protected:
  45                virtual void SendStart() = 0;
  46                virtual void SendStop() = 0;
  47                virtual void SetSpeed( int AIndex, float ASpeed ) = 0;
  48
  49        protected:
  50                void DoMotor1Receive( void *_Data )
  51                {
  52                        SetSpeed( 0, *(float *)_Data );
  53                }
  54
  55                void DoMotor2Receive( void *_Data )
  56                {
  57                        SetSpeed( 1, *(float *)_Data );
  58                }
  59
  60        public:
  61                SabertoothDualMotorBasic( Mitov::BasicSerialPort &ASerial ) :
  62                        FSerial( ASerial )
  63                {
  64                        MotorsInputPins[ 0 ].SetCallback( this, (OpenWire::TOnPinReceive)&SabertoothDualMotorBasic::DoMotor1Receive );
  65                        MotorsInputPins[ 1 ].SetCallback( this, (OpenWire::TOnPinReceive)&SabertoothDualMotorBasic::DoMotor2Receive );
  66                }
  67
  68        };
  69//---------------------------------------------------------------------------
  70        class SabertoothDualMotorSimple : public SabertoothDualMotorBasic
  71        {
  72                typedef SabertoothDualMotorBasic inherited;
  73
  74        public:
  75                OpenWire::SourcePin     SlaveSelectOutputPin;
  76
  77        protected:
  78                virtual void SendStart()
  79                {
  80                        float AValue = FSpeeds[ 0 ];
  81                        FSpeeds[ 0 ] = -1;
  82                        SetSpeed( 0, AValue );
  83
  84                        AValue = FSpeeds[ 1 ];
  85                        FSpeeds[ 1 ] = -1;
  86                        SetSpeed( 1, AValue );
  87                }
  88
  89                virtual void SendStop()
  90                {
  91                        SendValue( 0 );
  92                }
  93
  94                virtual void SetSpeed( int AIndex, float ASpeed )
  95                {
  96                        ASpeed = constrain( ASpeed, 0, 1 );
  97                        if( FSpeeds[ AIndex ] == ASpeed )
  98                                return;
  99
 100                        FSpeeds[ AIndex ] = ASpeed;
 101                        if( ! Enabled )
 102                                return;
 103
 104                        byte AValue = ASpeed * 126;
 105
 106                        if( AIndex )
 107                                AValue += 128;
 108
 109                        else
 110                                ++ AValue;
 111
 112                        SendValue( AValue );
 113                }
 114
 115                void SendValue( byte AValue )
 116                {
 117                        SlaveSelectOutputPin.SendValue( true );
 118
 119                        FSerial.GetStream().write( AValue );
 120
 121                        if( SlaveSelectOutputPin.IsConnected() )
 122                        {
 123                                delayMicroseconds( 50 );
 124                                SlaveSelectOutputPin.SendValue( false );
 125                        }
 126                }
 127
 128        protected:
 129                virtual void SystemStart() override
 130                {
 131                        SlaveSelectOutputPin.SendValue( false );
 132                        inherited::SystemStart();
 133                }
 134
 135        public:
 136                using inherited::inherited;
 137
 138        };
 139//---------------------------------------------------------------------------
 140        class SabertoothDualMotorPacket : public SabertoothDualMotorBasic
 141        {
 142                typedef SabertoothDualMotorBasic inherited;
 143
 144/*
 145        public:
 146                class Motor : public OpenWire::Object
 147                {
 148                public:
 149                        OpenWire::SinkPin       InputPin;
 150                protected:
 151                        bool FEnabled;
 152
 153                protected:
 154                        SabertoothDualMotorPacket &FOwner;
 155                        int     FIndex;
 156                        float FSpeed;
 157
 158                public:
 159                        void SetEnabled( bool AValue )
 160                        {
 161                                if( AValue == FEnabled )
 162                                        return;
 163
 164                                FEnabled = AValue;
 165                                if( FEnabled )
 166                                        SendStart();
 167
 168                                else
 169                                        SendStop();
 170                        }
 171
 172                protected:
 173                        virtual void SendStart()
 174                        {
 175                                byte  AValue = FSpeed * 255 + 0.5;
 176                                byte  ACommand = FIndex << 2; // 0 or 4
 177                                if( ( AValue & B10000000 ) == 0 )
 178                                        ACommand |= 1;
 179
 180                                FOwner.SendCommand( ACommand, AValue & B01111111 );
 181                        }
 182
 183                        virtual void SendStop()
 184                        {
 185                        }
 186
 187                protected:
 188                        void DoMotorReceive( void *_Data )
 189                        {
 190                                float ASpeed = constrain( *(float *)_Data, 0, 1 );
 191                                if( ASpeed == FSpeed )
 192                                        return;
 193
 194                                FSpeed = ASpeed;
 195                                if( Enabled )
 196                                        SendStart();
 197                        }
 198
 199                public:
 200                        Motor( SabertoothDualMotorPacket &AOwner, int AIndex ) : 
 201                                FOwner( AOwner ),
 202                                FIndex( AIndex ),
 203                                FEnabled( true )
 204                        {
 205                                InputPin.SetCallback( this, (OpenWire::TOnPinReceive)&Motor::DoMotorReceive );
 206                        }
 207                };
 208*/
 209        public:
 210                byte    Address = 128;
 211
 212                float   MinVoltage = 6.0f;
 213                float   MaxVoltage = 25.0;
 214
 215                byte    Ramping = 0;
 216
 217                bool    DirectionalMode = false;
 218                uint16_t        SerialTimeout = 0;
 219                byte    Deadband = 0;
 220
 221        public:
 222                void SetMinVoltage( float AValue )
 223                {
 224                        AValue = constrain( AValue, 6.0, 30.0 );
 225                        if( MinVoltage == AValue )
 226                                return;
 227
 228                        MinVoltage = AValue;
 229                        SendCommand( 2, ( MinVoltage - 6 ) * 5 );
 230                }
 231
 232                void SetMaxVoltage( float AValue )
 233                {
 234                        AValue = constrain( AValue, 0.0, 25.0 );
 235                        if( MaxVoltage == AValue )
 236                                return;
 237
 238                        MaxVoltage = AValue;
 239                        SendCommand( 3, MinVoltage * 5.12 );
 240                }
 241
 242                void SetRamping( byte AValue )
 243                {
 244                        AValue = constrain( AValue, 0, 80 );
 245                        if( Ramping == AValue )
 246                                return;
 247
 248                        Ramping = AValue;
 249                        SendCommand( 16, Ramping );
 250                }
 251
 252                void SetSerialTimeout( uint16_t AValue )
 253                {
 254                        AValue = constrain( AValue, 0, 12700 );
 255                        if( SerialTimeout == AValue )
 256                                return;
 257
 258                        SerialTimeout = AValue;
 259                        SendSerialTimeout();
 260                }
 261
 262                void SetDeadband( byte AValue )
 263                {
 264                        AValue = constrain( AValue, 0, 127 );
 265                        if( Deadband == AValue )
 266                                return;
 267
 268                        Deadband = AValue;
 269                        SendCommand( 17, Deadband );
 270                }
 271
 272        protected:
 273                void SendSerialTimeout()
 274                {
 275                        SendCommand( 14, (SerialTimeout + 99) / 100 );
 276                }
 277
 278                virtual void SendStart()
 279                {
 280                        SendStartOne( 0 );
 281                        SendStartOne( 1 );
 282                }
 283
 284                void SendStartOne( int AIndex )
 285                {
 286                        int  AValue = ( FSpeeds[ AIndex ] * 255 + 0.5 ) - 128;
 287                        byte  ACommand;
 288                        if( DirectionalMode )
 289                        {
 290                                if( AIndex == 0 )
 291                                        ACommand = 10;
 292
 293                                else
 294                                        ACommand = 8;
 295                        }
 296
 297                        else
 298                                ACommand = AIndex << 2; // 0 or 4
 299
 300                        if( AValue < 0 )
 301                        {
 302                                AValue = -AValue;
 303                                ACommand |= 1;
 304                        }
 305
 306                        SendCommand( ACommand, AValue & B01111111 );
 307                }
 308
 309                virtual void SendStop()
 310                {
 311                        // Shut down both motors
 312                        SendCommand( 0, 0 );
 313                        SendCommand( 4, 0 );
 314                }
 315
 316                virtual void SetSpeed( int AIndex, float ASpeed )
 317                {
 318                        ASpeed = constrain( ASpeed, 0, 1 );
 319                        if( ASpeed == FSpeeds[ AIndex ] )
 320                                return;
 321
 322                        FSpeeds[ AIndex ] = ASpeed;
 323                        if( Enabled )
 324                                SendStart();
 325                }
 326
 327        protected:
 328                void SendCommand(byte ACommand, byte AValue)
 329                {
 330                        Stream &AStream = FSerial.GetStream();
 331
 332                        byte AAddress = Address | B10000000;
 333                        AStream.write( AAddress );
 334                        AStream.write( ACommand );
 335                        AStream.write( AValue );
 336                        AStream.write(( AAddress + ACommand + AValue ) & B01111111);
 337                }
 338
 339        protected:
 340                virtual void SystemStart() 
 341                {
 342                        if( MinVoltage != 6.0 )
 343                                SendCommand( 2, ( MinVoltage - 6 ) * 5 );
 344
 345                        if( MaxVoltage != 25.0 )
 346                                SendCommand( 3, MinVoltage * 5.12 );
 347
 348                        if( Ramping )
 349                                SendCommand( 16, Ramping );
 350
 351                        if( SerialTimeout )
 352                                SendSerialTimeout();
 353
 354                        if( Deadband )
 355                                SendCommand( 17, Deadband );
 356
 357                        inherited::SystemStart();
 358                }
 359
 360        public:
 361                using inherited::inherited;
 362
 363        };
 364//---------------------------------------------------------------------------
 365}
 366
 367#endif