File content as of revision 0:d19dd72afbb0:

/** K9 Dr Who Robot dog, Head controler.
* Based on NucleoF303k8 CAN Receive node
 *
 *
 *
 *
 *
 */

#include "mbed.h"
#include "MultiChannelRelay.h"
//#include "PCA9685.h"
#include "Adafruit_PWMServoDriver.h"

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

#define NODE_ID 0x456
#define NODE_RELAY 0x81
#define NODE_MOTOR 0x82
#define NODE_SERVO 0x83


#define WAVE_MIN  120
#define WAVE_MAX  450
#define WAVE_FWD  285
#define RIGHT_EAR 0
#define LEFT_EAR  1
#define GUN_SERVO 2
#define EYE_LEDS  3

// Blinking rate in milliseconds
#define BLINKING_RATE     500

I2C i2c(I2C_SDA, I2C_SCL);

//PCA9685 pwmServo( I2C_SDA, I2C_SCL, PCA9685::PCA9685_ADDRESS_0, 400000 );
Adafruit_PWMServoDriver pwmServo( &i2c );

void canRcv(void);
char RcvData[8];
DigitalOut led(PB_3);
//Serial pc(PA_2,PA_3,115200);
CAN can(PA_11, PA_12,500000);       // Rx, Tx
CANMessage msg(0x456,CANStandard);

//I2C i2c(I2C_SDA, I2C_SCL);

MultiChannelRelay relay(&i2c);


/** CAN **/
void canRcv(void)
{
    led =!led;
    can.read(msg);
//    printf("\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];
    }
    /*    printf("\033[2;1HCAN DATA[0]: 0x%x ", RcvData[0]);
        printf("\033[3;1HCAN DATA[1]: 0x%x ", RcvData[1]);
        printf("\033[4;1HCAN DATA[2]: 0x%x ", RcvData[2]);
        printf("\033[5;1HCAN DATA[3]: 0x%x ", RcvData[3]);
        printf("\033[6;1HCAN DATA[4]: 0x%x ", RcvData[4]);
        printf("\033[7;1HCAN DATA[5]: 0x%x ", RcvData[5]);
        printf("\033[8;1HCAN DATA[6]: 0x%x ", RcvData[6]);
        printf("\033[9;1HCAN DATA[7]: 0x%x ", RcvData[7]);
        printf("\033[10;1HCAN LEN:    %d  ",msg.len);
        printf("\033[11;1HCAN FORMAT: %d  ",msg.format);
        printf("\033[12;1HCAN TYPE:   %d  ",msg.type);
        printf("\033[13;1HCAN RDErr:  %d  ",can.rderror());
    */
    /*
        // Byte 0 is node type
        switch( RcvData[0] ) {
            case NODE_RELAY:
                // Byte 1 is relay bit pattern
                relay.channelCtrl( RcvData[1] & 0x0F );
                printf("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
                break;
        }
    */
//    }
}


int main()
{
    // Setup hardware

    //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

#ifdef NOTUSED
    printf("\033[2J\033[1;1HTest Relays\n");

    // Relay test
    relay.channelCtrl(0x00);
    thread_sleep_for(100);
    relay.channelCtrl(0x01);
    thread_sleep_for(100);
    relay.channelCtrl(0x02);
    thread_sleep_for(100);
    relay.channelCtrl(0x04);
    thread_sleep_for(100);
    relay.channelCtrl(0x08);
    thread_sleep_for(100);
    relay.channelCtrl(0x00);

    // Servo test
    printf("\033[2J\033[1;1HTest PWM\n");
    for(int i=WAVE_MIN; i<WAVE_MAX; ) {
        //status = myPWMSensor.PCA9685_SetPWM_DutyCycle( PCA9685::PCA9685_LED1, 10, 30 );
        pwmServo.setPWM(RIGHT_EAR, 0, i);
        thread_sleep_for(20);
        i += 10;
    }
    for(int i=WAVE_MAX; i>WAVE_MIN; ) {
        //status = myPWMSensor.PCA9685_SetPWM_DutyCycle( PCA9685::PCA9685_LED1, 10, 30 );
        pwmServo.setPWM(RIGHT_EAR, 0, i);
        thread_sleep_for(20);
        i -= 10;
    }
    for( int i=1; i<5; i++ ) {
        pwmServo.setPWM(RIGHT_EAR, 0, WAVE_MIN);
        pwmServo.setPWM(LEFT_EAR, 0, WAVE_MAX);
        thread_sleep_for(700);
        pwmServo.setPWM(RIGHT_EAR, 0, WAVE_MAX);
        pwmServo.setPWM(LEFT_EAR, 0, WAVE_MIN);
        thread_sleep_for(700);
    }

    for( int n=0; n<10; n++) {
        pwmServo.setPWM(EYE_LEDS, 0, 0xFFF);
        thread_sleep_for(500);
        pwmServo.setPWM(EYE_LEDS, 0, 0);
        thread_sleep_for(500);
        /*
                for(int i=0; i<0xFFFF; ) {
                    //status = myPWMSensor.PCA9685_SetPWM_DutyCycle( PCA9685::PCA9685_LED1, 10, 30 );
                    pwmServo.setPWM(EYE_LEDS, 0, i);
                    thread_sleep_for(10);
                    i += 10;
                }
                for(int i=0xFFFF; i>0; ) {
                    //status = myPWMSensor.PCA9685_SetPWM_DutyCycle( PCA9685::PCA9685_LED1, 10, 30 );
                    pwmServo.setPWM(EYE_LEDS, 0, i);
                    thread_sleep_for(10);
                    i -= 10;
                }
         */
    }

#endif

    relay.channelCtrl(0x00);
    pwmServo.setPWM(RIGHT_EAR, 0, WAVE_FWD);
    pwmServo.setPWM(LEFT_EAR, 0, WAVE_FWD);

    pwmServo.setPWM(EYE_LEDS, 0, 0);

    printf("\033[2J\033[1;1HSTART mbed 6.2\n");

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

//    while (true) {
//        led = !led;
//        thread_sleep_for(500);
//    }

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

    while(1) {
        led = !led;
        if( can.read(msg) > 0 ) {
//    printf("\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];
            }

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

            // Byte 0 is node type
            switch( RcvData[0] ) {
                case NODE_RELAY:
                    // Byte 1 is relay bit pattern
                    relay.channelCtrl( RcvData[1] & 0x0F );
                    //printf("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);
    }

}