SMC02B.cpp

00001 /**
00002 * Includes
00003 */
00004 #include "SMC02B.h" 
00005  
00006     SMC02B::SMC02B(PinName tx, PinName rx, PinName rst) : _rst(rst)
00007     {
00008         _SMC02B = new Serial( tx, rx);
00009         _rst = 1;
00010     }
00011 
00012     void SMC02B::SMC02B_RST(void)
00013     {
00014         _rst = 1;
00015         wait(0.1);
00016         _rst = 0;
00017         wait(0.1);
00018         _rst = 1;
00019         wait(0.1);
00020     }
00021 
00022     // Set Motor movement
00023     // motor
00024     // motor ID
00025     // direction
00026     // 0 - Reverse
00027     // 1 - Forward
00028     // speed
00029     // 0 - 127 ( 0x7F )
00030     void SMC02B::SMC02B_MOTOR_SET( char motor, char direction, char speed )
00031     {
00032         char motor_ = motor << 1;
00033         _SMC02B -> putc( SMC02B_START_BYTE );
00034         _SMC02B -> putc( SMC02B_DEVICE_ID );
00035         _SMC02B -> putc( motor_ | direction );    
00036         _SMC02B -> putc( speed );
00037     }
00038 
00039     // OFF Backward -> Brake
00040     void SMC02B::SMC02B_MOTOR_BRAKE( char motor )
00041     {
00042         char motor_ = motor << 1;
00043         _SMC02B -> putc( SMC02B_START_BYTE );
00044         _SMC02B -> putc( SMC02B_DEVICE_ID );
00045         _SMC02B -> putc( motor_ | 0x00 );    
00046         _SMC02B -> putc( 0x00 );
00047     }
00048 
00049     // OFF Forward -> Coasting
00050     void SMC02B::SMC02B_MOTOR_COAST( char motor )
00051     {
00052         char motor_ = motor << 1;
00053         _SMC02B -> putc( SMC02B_START_BYTE );
00054         _SMC02B -> putc( SMC02B_DEVICE_ID );
00055         _SMC02B -> putc( motor_ | 0x01 );    
00056         _SMC02B -> putc( 0x00 );
00057     }
00058 
00059     // Config
00060     // mode 
00061     // 0 - 2 Motors
00062     // 1 - 1 Motor
00063     // motor
00064     // 0 - 63 ( 0x3F ) First motor number
00065     void SMC02B::SMC02B_CONFIG( char mode, char motor )
00066     {
00067         char mode_ = mode << 6;
00068         char motor_ = motor & 0x3F;
00069         _SMC02B -> putc( SMC02B_START_BYTE );
00070         _SMC02B -> putc( SMC02B_CONFIG_BYTE );
00071         _SMC02B -> putc(mode_ | motor_ );    
00072     }
00073 
00074 
00075