main.cpp

00001 /** K9 Dr Who Robot dog, Head controler.
00002 * Based on NucleoF303k8 CAN Receive node
00003  *"requires": ["bare-metal","events"],
00004  *
00005  *
00006  * Body controller - Implements Tail, Head nod and Probe.
00007  *
00008  * Function           Byte 0  Byte 1  Byte 2  Byte 3  Byte 4  Byte 5  Byte 6  Byte 7
00009  * Nose Gun Extend     0x01    0x01
00010  * Nose Gun Retract    0x01    0x00
00011  * Waggle ears         0x02    n
00012  * Eye LEDs fade on    0x03    0x01
00013  * Eye LEDs off        0x03    0x00
00014  * Wag tail left/right 0x04    0x01    n
00015  * Raise tail          0x04    0x02
00016  * Lower tail          0x04    0x03
00017  * Centre tail         0x04    0x00
00018  * Move Head up        0x05    0x01
00019  * Move Head down      0x05    0x02
00020  * Probe Extend        0x06    0x01
00021  * Probe Retract       0x06    0x00
00022  *
00023  * Relay board         0x81    bit pattern
00024  * Motor Control       0x82
00025  * PWM Control         0x83
00026  *
00027  */
00028 
00029 #include "mbed.h"
00030 #include "mbed_events.h"
00031 //#include <stdio.h>
00032 #include "Adafruit_PWMServoDriver.h"
00033 
00034 //     "requires": ["bare-metal"],
00035 
00036 #define NODE_ID 0x456
00037 
00038 // Define controller operations
00039 #define K9_HEAD_GUN   0x01
00040 #define K9_HEAD_EARS  0x02
00041 #define K9_HEAD_EYES  0x03
00042 #define K9_TAIL       0x04
00043 #define K9_HEAD       0x05
00044 #define K9_PROBE      0x06
00045 
00046 // Specific I/O functions
00047 #define NODE_RELAY 0x81
00048 #define NODE_MOTOR 0x82
00049 #define NODE_SERVO 0x83
00050 
00051 
00052 // Servo numbers
00053 #define RIGHT_EAR  0
00054 #define LEFT_EAR   1
00055 #define GUN_SERVO  2
00056 #define EYE_LEDS   3
00057 #define TAIL_HORIZ 4
00058 #define TAIL_VERT  5
00059 
00060 // GPIO
00061 #define PROBE_RELAY PF_1
00062 #define HEAD_UPPER_LIMIT PB_4
00063 #define HEAD_LOWER_LIMIT PB_5
00064 #define HEAD_MOTOR1 PB_0
00065 #define HEAD_MOTOR2 PB_1
00066 
00067 // Servo parameters
00068 #define WAVE_MIN  120
00069 #define WAVE_MAX  450
00070 #define WAVE_FWD  285
00071 
00072 #define EYE_MAX   4095
00073 #define EYE_MIN   0
00074 
00075 #define WAG_MIN  220
00076 #define WAG_MAX  450
00077 #define TAIL_VERT_MIN  220
00078 #define TAIL_VERT_MAX  450
00079 #define TAIL_CENTRE_HORIZ  285
00080 #define TAIL_CENTRE_VERT  320
00081 
00082 // Blinking rate in milliseconds
00083 #define BLINKING_RATE     500
00084 
00085 // Instantiate I2C and servo objects
00086 I2C i2c(PB_7, PB_6);
00087 //I2C i2c(I2C_SDA, I2C_SCL);
00088 Adafruit_PWMServoDriver pwmServo( &i2c );
00089 
00090 void canRcv(void);
00091 char RcvData[8];
00092 DigitalOut led(PB_3);
00093 DigitalOut probeRelay( PROBE_RELAY, 0 );
00094 DigitalOut headMotor1( HEAD_MOTOR1, 0 );
00095 DigitalOut headMotor2( HEAD_MOTOR2, 0 );
00096 DigitalIn headUpperLimit( HEAD_UPPER_LIMIT, PullUp );
00097 DigitalIn headLowerLimit( HEAD_LOWER_LIMIT, PullUp );
00098 
00099 //Serial pc(PA_2,PA_3,115200);
00100 CAN can(PA_11, PA_12,500000);       // Rx, Tx
00101 CANMessage msg(NODE_ID,CANStandard);
00102 
00103 
00104 /** CAN **/
00105 void canRcv(void)
00106 {
00107     led =!led;
00108     can.read(msg);
00109 //    debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
00110 //    if(msg.id == NODE_ID) {
00111     for(uint8_t i=0; i<8; i++) {
00112         RcvData[i] = msg.data[i];
00113     }
00114 }
00115 
00116 // Movement functions
00117 
00118 void probeExtend( void )
00119 {
00120     debug("\033[12;1HProbe Extend     \n");
00121     probeRelay = 1;
00122 }
00123 
00124 void probeRetract( void )
00125 {
00126     debug("\033[12;1HProbe Retract    \n");
00127     probeRelay = 0;
00128 }
00129 
00130 void tailCentre()
00131 {
00132     debug("\033[12;1HTail Centre     \n");
00133     pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
00134     pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
00135 }
00136 
00137 void tailWag( uint8_t numWags )
00138 {
00139     debug("\033[12;HWag Tail %d      \n", numWags);
00140     pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
00141     pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
00142     for( int i=0; i< numWags ; i++ ) {
00143         pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MIN);
00144         thread_sleep_for(700);
00145         pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MAX);
00146         thread_sleep_for(700);
00147     }
00148     tailCentre();
00149 }
00150 
00151 void tailRaise()
00152 {
00153     debug("\033[12;1HTail Raise     \n");
00154     pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MAX);
00155 }
00156 
00157 void tailLower()
00158 {
00159     debug("\033[12;1HTail Lower     \n");
00160     pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MIN);
00161 }
00162 
00163 void linearMotorOff()
00164 {
00165     headMotor1.write(0);
00166     headMotor2.write(0);
00167 }
00168 
00169 void linearMotorUp()
00170 {
00171     headMotor1.write(0);
00172     headMotor2.write(1);
00173 }
00174 
00175 void linearMotorDown()
00176 {
00177     headMotor1.write(1);
00178     headMotor2.write(0);
00179 }
00180 
00181 void headRaiseFull()
00182 {
00183     debug("\033[12;1HHead Raise Full     \n");
00184     // Motor to be driven down, check lower limit
00185 
00186     // Check Lower limit switch
00187     if( headLowerLimit.read() == 0 )
00188         return;
00189 
00190     // Turn on Motor
00191     linearMotorDown();
00192 
00193     // Wait for limit
00194     while( headLowerLimit.read() != 0 )
00195         thread_sleep_for(5);
00196 
00197     // Turn off motor
00198     linearMotorOff();
00199 }
00200 
00201 void headRaise()
00202 {
00203     debug("\033[12;1HHead Raise     \n");
00204     // Motor to be driven down, check lower limit
00205 
00206     // Check Lower limit switch
00207     if( headLowerLimit.read() == 0 )
00208         return;
00209 
00210     // Turn on Motor
00211     linearMotorDown();
00212 
00213     // Wait for lower limit or time reached
00214     int timeCount = 200;
00215     while( headLowerLimit.read() != 0 && --timeCount > 0 )
00216         thread_sleep_for(5);
00217 
00218     // Turn off motor
00219     linearMotorOff();
00220 
00221 }
00222 
00223 void headLowerFull()
00224 {
00225     debug("\033[12;1HHead Lower Full     \n");
00226     // Motor to be driven up, check upper limit
00227 
00228     // Check Upper limit switch
00229     if( headUpperLimit.read() == 0 )
00230         return;
00231 
00232     // Turn on Motor
00233     linearMotorUp();
00234 
00235     // Wait for limit
00236     while( headUpperLimit.read() != 0 )
00237         thread_sleep_for(5);
00238 
00239     // Turn off motor
00240     linearMotorOff();
00241 }
00242 
00243 void headLower()
00244 {
00245     debug("\033[12;1HHead Lower      \n");
00246     // Motor to be driven up, check upper limit
00247 
00248     // Check Lower limit switch
00249     if( headLowerLimit.read() == 0 )
00250         return;
00251 
00252     // Turn on Motor
00253     linearMotorDown();
00254 
00255     // Wait for upper limit
00256     while( headLowerLimit.read() != 0 )
00257         thread_sleep_for(5);
00258 
00259     // Turn off motor
00260     linearMotorOff();
00261 }
00262 
00263 
00264 
00265 int main()
00266 {
00267     // Setup hardware
00268 
00269     headMotor1 = 0;
00270     headMotor2 = 0;
00271     //PCA9685::PCA9685_status_t  status = pwmServo.PCA9685_SoftReset();
00272     pwmServo.begin();
00273 
00274     // Configure the PWM frequency and wake up the device
00275     //status = pwmServo.PCA9685_SetPWM_Freq( 1000 );   // PWM frequency: 1kHz
00276     //status = pwmServo.PCA9685_SetMode( PCA9685::MODE1_SLEEP_DISABLED );
00277     pwmServo.setPWMFreq(50);  // 50Hz
00278 
00279     // creates a queue with the default size
00280 //    EventQueue queue;
00281 
00282     // Set initial positions of movable parts
00283     probeRetract();
00284     tailCentre();
00285     headRaiseFull();
00286 
00287 
00288     debug("\033[2J\033[1;1HSTART K9 Body Controller, ");
00289     debug("mbed-os: %d.%d.%d\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);
00290 
00291     // Initialise the digital pin LED1 as an output
00292     DigitalOut led(LED1);
00293 
00294 
00295     can.mode( CAN::Normal );
00296 //    can.attach(callback( &canRcv ),CAN::RxIrq);
00297 
00298     while(1) {
00299         debug("\033[13;1HLimits Upper %d, Lower %d    \n",headUpperLimit.read(), headLowerLimit.read() );
00300 //            led = !led;
00301         if( can.read(msg) > 0 ) {
00302             led = !led;
00303 //    debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
00304 //    if(msg.id == NODE_ID) {
00305             for(uint8_t i=0; i<8; i++) {
00306                 RcvData[i] = msg.data[i];
00307             }
00308 
00309             debug("\033[2;1HCAN DATA[0]: 0x%x", RcvData[0]);
00310             debug("\033[3;1HCAN DATA[1]: 0x%x", RcvData[1]);
00311             debug("\033[4;1HCAN DATA[2]: 0x%x", RcvData[2]);
00312             debug("\033[5;1HCAN DATA[3]: 0x%x", RcvData[3]);
00313             debug("\033[6;1HCAN DATA[4]: 0x%x", RcvData[4]);
00314             debug("\033[7;1HCAN DATA[5]: 0x%x", RcvData[5]);
00315             debug("\033[8;1HCAN DATA[6]: 0x%x", RcvData[6]);
00316             debug("\033[9;1HCAN DATA[7]: 0x%x", RcvData[7]);
00317 
00318             // Byte 0 is node type
00319             switch( RcvData[0] ) {
00320                 /*
00321                                 case K9_HEAD_GUN:       // Move Gun
00322                                     if( RcvData[1] == 0x01 )    // Extend gun
00323                                         gunExtend();
00324                                     else if( RcvData[1] == 0x00 )    // Retract gun
00325                                         gunRetract();
00326                                     break;
00327 
00328                                 case K9_HEAD_EARS:      // Move ears
00329                                     if( RcvData[1] > 0x00 )     // Ear scan
00330                                         earScan( RcvData[1] );
00331                                     break;
00332 
00333                                 case K9_HEAD_EYES:      // Flash/fade eyes
00334                                     if( RcvData[1] > 0x00 )     // Fade eyes
00335                                         eyesStartFade();
00336                                     else
00337                                         eyesStopFade();
00338 
00339                                     break;
00340                 */
00341                 case K9_TAIL:           // Move Tail
00342                     if( RcvData[1] == 0x01 )    // Wag Tail
00343                         tailWag(RcvData[2]);
00344                     else if( RcvData[1] == 0x02 )    // Raise Tail
00345                         tailRaise();
00346                     else if( RcvData[1] == 0x03 )    // Lower Tail
00347                         tailLower();
00348                     else
00349                         tailCentre();
00350                     break;
00351 
00352                 case K9_HEAD:       // Move Head up/down
00353                     if( RcvData[1] == 0x01 )    //Raise Head
00354                         headRaiseFull();
00355                     else if( RcvData[1] == 0x02 )    // Lower Head
00356                         headLowerFull();
00357                     break;
00358 
00359                 case K9_PROBE:       // Move Probe
00360                     if( RcvData[1] == 0x01 )    // Extend Probe
00361                         probeExtend();
00362                     else if( RcvData[1] == 0x00 )    // Retract Probe
00363                         probeRetract();
00364                     break;
00365                 /*
00366                                 case NODE_RELAY:
00367                                     // Byte 1 is relay bit pattern
00368                                     relay.channelCtrl( RcvData[1] & 0x0F );
00369                                     //debug("Relay %d  \r\n", RcvData[1] & 0x0F );
00370                                     break;
00371 
00372                 case NODE_MOTOR:
00373                     // Bytes 1/2 L/H signed int Right direction/speed
00374                     // Bytes 3/4 L/H signed int Left direction/speed
00375                     break;
00376                 */
00377                 case NODE_SERVO:
00378                     // Byte 1 servo number
00379                     // Byte 2/3 L/H value
00380                     uint16_t servoVal = (RcvData[3] << 8) | RcvData[2];
00381                     pwmServo.setPWM(RcvData[1] & 0x0F, 0, servoVal );
00382                     break;
00383 
00384 //                default:    // Reset data
00385 //                    RcvData[0] = 0;
00386 //                    break;
00387 
00388             }
00389             RcvData[0] = 0;
00390         }
00391         thread_sleep_for(100);
00392     }
00393 
00394 }