File content as of revision 1:558786a5d3f9:

/** K9 Dr Who Robot dog, Head controler.
* Based on NucleoF303k8 CAN Receive node
 *"requires": ["bare-metal","events"],
 *
 *
 * Body controller - Implements Tail, Head nod and Probe.
 *
 * Function           Byte 0  Byte 1  Byte 2  Byte 3  Byte 4  Byte 5  Byte 6  Byte 7
 * Nose Gun Extend     0x01    0x01
 * Nose Gun Retract    0x01    0x00
 * Waggle ears         0x02    n
 * Eye LEDs fade on    0x03    0x01
 * Eye LEDs off        0x03    0x00
 * Wag tail left/right 0x04    0x01    n
 * Raise tail          0x04    0x02
 * Lower tail          0x04    0x03
 * Centre tail         0x04    0x00
 * Move Head up        0x05    0x01
 * Move Head down      0x05    0x02
 * Probe Extend        0x06    0x01
 * Probe Retract       0x06    0x00
 *
 * Relay board         0x81    bit pattern
 * Motor Control       0x82
 * PWM Control         0x83
 *
 */

#include "mbed.h"
#include "mbed_events.h"
//#include <stdio.h>
#include "Adafruit_PWMServoDriver.h"

//     "requires": ["bare-metal"],

#define NODE_ID 0x456

// Define controller operations
#define K9_HEAD_GUN   0x01
#define K9_HEAD_EARS  0x02
#define K9_HEAD_EYES  0x03
#define K9_TAIL       0x04
#define K9_HEAD       0x05
#define K9_PROBE      0x06

// Specific I/O functions
#define NODE_RELAY 0x81
#define NODE_MOTOR 0x82
#define NODE_SERVO 0x83


// Servo numbers
#define RIGHT_EAR  0
#define LEFT_EAR   1
#define GUN_SERVO  2
#define EYE_LEDS   3
#define TAIL_HORIZ 4
#define TAIL_VERT  5

// GPIO
#define PROBE_RELAY PF_1
#define HEAD_UPPER_LIMIT PB_4
#define HEAD_LOWER_LIMIT PB_5
#define HEAD_MOTOR1 PB_0
#define HEAD_MOTOR2 PB_1

// Servo parameters
#define WAVE_MIN  120
#define WAVE_MAX  450
#define WAVE_FWD  285

#define EYE_MAX   4095
#define EYE_MIN   0

#define WAG_MIN  220
#define WAG_MAX  450
#define TAIL_VERT_MIN  220
#define TAIL_VERT_MAX  450
#define TAIL_CENTRE_HORIZ  285
#define TAIL_CENTRE_VERT  320

// Blinking rate in milliseconds
#define BLINKING_RATE     500

// Instantiate I2C and servo objects
I2C i2c(PB_7, PB_6);
//I2C i2c(I2C_SDA, I2C_SCL);
Adafruit_PWMServoDriver pwmServo( &i2c );

void canRcv(void);
char RcvData[8];
DigitalOut led(PB_3);
DigitalOut probeRelay( PROBE_RELAY, 0 );
DigitalOut headMotor1( HEAD_MOTOR1, 0 );
DigitalOut headMotor2( HEAD_MOTOR2, 0 );
DigitalIn headUpperLimit( HEAD_UPPER_LIMIT, PullUp );
DigitalIn headLowerLimit( HEAD_LOWER_LIMIT, PullUp );

//Serial pc(PA_2,PA_3,115200);
CAN can(PA_11, PA_12,500000);       // Rx, Tx
CANMessage msg(NODE_ID,CANStandard);


/** CAN **/
void canRcv(void)
{
    led =!led;
    can.read(msg);
//    debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
//    if(msg.id == NODE_ID) {
    for(uint8_t i=0; i<8; i++) {
        RcvData[i] = msg.data[i];
    }
}

// Movement functions

void probeExtend( void )
{
    debug("\033[12;1HProbe Extend     \n");
    probeRelay = 1;
}

void probeRetract( void )
{
    debug("\033[12;1HProbe Retract    \n");
    probeRelay = 0;
}

void tailCentre()
{
    debug("\033[12;1HTail Centre     \n");
    pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
    pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
}

void tailWag( uint8_t numWags )
{
    debug("\033[12;HWag Tail %d      \n", numWags);
    pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
    pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
    for( int i=0; i< numWags ; i++ ) {
        pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MIN);
        thread_sleep_for(700);
        pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MAX);
        thread_sleep_for(700);
    }
    tailCentre();
}

void tailRaise()
{
    debug("\033[12;1HTail Raise     \n");
    pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MAX);
}

void tailLower()
{
    debug("\033[12;1HTail Lower     \n");
    pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MIN);
}

void linearMotorOff()
{
    headMotor1.write(0);
    headMotor2.write(0);
}

void linearMotorUp()
{
    headMotor1.write(0);
    headMotor2.write(1);
}

void linearMotorDown()
{
    headMotor1.write(1);
    headMotor2.write(0);
}

void headRaiseFull()
{
    debug("\033[12;1HHead Raise Full     \n");
    // Motor to be driven down, check lower limit

    // Check Lower limit switch
    if( headLowerLimit.read() == 0 )
        return;

    // Turn on Motor
    linearMotorDown();

    // Wait for limit
    while( headLowerLimit.read() != 0 )
        thread_sleep_for(5);

    // Turn off motor
    linearMotorOff();
}

void headRaise()
{
    debug("\033[12;1HHead Raise     \n");
    // Motor to be driven down, check lower limit

    // Check Lower limit switch
    if( headLowerLimit.read() == 0 )
        return;

    // Turn on Motor
    linearMotorDown();

    // Wait for lower limit or time reached
    int timeCount = 200;
    while( headLowerLimit.read() != 0 && --timeCount > 0 )
        thread_sleep_for(5);

    // Turn off motor
    linearMotorOff();

}

void headLowerFull()
{
    debug("\033[12;1HHead Lower Full     \n");
    // Motor to be driven up, check upper limit

    // Check Upper limit switch
    if( headUpperLimit.read() == 0 )
        return;

    // Turn on Motor
    linearMotorUp();

    // Wait for limit
    while( headUpperLimit.read() != 0 )
        thread_sleep_for(5);

    // Turn off motor
    linearMotorOff();
}

void headLower()
{
    debug("\033[12;1HHead Lower      \n");
    // Motor to be driven up, check upper limit

    // Check Lower limit switch
    if( headLowerLimit.read() == 0 )
        return;

    // Turn on Motor
    linearMotorDown();

    // Wait for upper limit
    while( headLowerLimit.read() != 0 )
        thread_sleep_for(5);

    // Turn off motor
    linearMotorOff();
}



int main()
{
    // Setup hardware

    headMotor1 = 0;
    headMotor2 = 0;
    //PCA9685::PCA9685_status_t  status = pwmServo.PCA9685_SoftReset();
    pwmServo.begin();

    // Configure the PWM frequency and wake up the device
    //status = pwmServo.PCA9685_SetPWM_Freq( 1000 );   // PWM frequency: 1kHz
    //status = pwmServo.PCA9685_SetMode( PCA9685::MODE1_SLEEP_DISABLED );
    pwmServo.setPWMFreq(50);  // 50Hz

    // creates a queue with the default size
//    EventQueue queue;

    // Set initial positions of movable parts
    probeRetract();
    tailCentre();
    headRaiseFull();


    debug("\033[2J\033[1;1HSTART K9 Body Controller, ");
    debug("mbed-os: %d.%d.%d\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);

    // Initialise the digital pin LED1 as an output
    DigitalOut led(LED1);


    can.mode( CAN::Normal );
//    can.attach(callback( &canRcv ),CAN::RxIrq);

    while(1) {
        debug("\033[13;1HLimits Upper %d, Lower %d    \n",headUpperLimit.read(), headLowerLimit.read() );
//            led = !led;
        if( can.read(msg) > 0 ) {
            led = !led;
//    debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
//    if(msg.id == NODE_ID) {
            for(uint8_t i=0; i<8; i++) {
                RcvData[i] = msg.data[i];
            }

            debug("\033[2;1HCAN DATA[0]: 0x%x", RcvData[0]);
            debug("\033[3;1HCAN DATA[1]: 0x%x", RcvData[1]);
            debug("\033[4;1HCAN DATA[2]: 0x%x", RcvData[2]);
            debug("\033[5;1HCAN DATA[3]: 0x%x", RcvData[3]);
            debug("\033[6;1HCAN DATA[4]: 0x%x", RcvData[4]);
            debug("\033[7;1HCAN DATA[5]: 0x%x", RcvData[5]);
            debug("\033[8;1HCAN DATA[6]: 0x%x", RcvData[6]);
            debug("\033[9;1HCAN DATA[7]: 0x%x", RcvData[7]);

            // Byte 0 is node type
            switch( RcvData[0] ) {
                /*
                                case K9_HEAD_GUN:       // Move Gun
                                    if( RcvData[1] == 0x01 )    // Extend gun
                                        gunExtend();
                                    else if( RcvData[1] == 0x00 )    // Retract gun
                                        gunRetract();
                                    break;

                                case K9_HEAD_EARS:      // Move ears
                                    if( RcvData[1] > 0x00 )     // Ear scan
                                        earScan( RcvData[1] );
                                    break;

                                case K9_HEAD_EYES:      // Flash/fade eyes
                                    if( RcvData[1] > 0x00 )     // Fade eyes
                                        eyesStartFade();
                                    else
                                        eyesStopFade();

                                    break;
                */
                case K9_TAIL:           // Move Tail
                    if( RcvData[1] == 0x01 )    // Wag Tail
                        tailWag(RcvData[2]);
                    else if( RcvData[1] == 0x02 )    // Raise Tail
                        tailRaise();
                    else if( RcvData[1] == 0x03 )    // Lower Tail
                        tailLower();
                    else
                        tailCentre();
                    break;

                case K9_HEAD:       // Move Head up/down
                    if( RcvData[1] == 0x01 )    //Raise Head
                        headRaiseFull();
                    else if( RcvData[1] == 0x02 )    // Lower Head
                        headLowerFull();
                    break;

                case K9_PROBE:       // Move Probe
                    if( RcvData[1] == 0x01 )    // Extend Probe
                        probeExtend();
                    else if( RcvData[1] == 0x00 )    // Retract Probe
                        probeRetract();
                    break;
                /*
                                case NODE_RELAY:
                                    // Byte 1 is relay bit pattern
                                    relay.channelCtrl( RcvData[1] & 0x0F );
                                    //debug("Relay %d  \r\n", RcvData[1] & 0x0F );
                                    break;

                case NODE_MOTOR:
                    // Bytes 1/2 L/H signed int Right direction/speed
                    // Bytes 3/4 L/H signed int Left direction/speed
                    break;
                */
                case NODE_SERVO:
                    // Byte 1 servo number
                    // Byte 2/3 L/H value
                    uint16_t servoVal = (RcvData[3] << 8) | RcvData[2];
                    pwmServo.setPWM(RcvData[1] & 0x0F, 0, servoVal );
                    break;

//                default:    // Reset data
//                    RcvData[0] = 0;
//                    break;

            }
            RcvData[0] = 0;
        }
        thread_sleep_for(100);
    }

}