K9 Head controlled uses CAN communications, drives ear servos, gun servo and eyes.
Dependencies: MultiChannelRelay Adafruit_PWMServoDriver
Diff: main.cpp
- Revision:
- 0:d19dd72afbb0
- Child:
- 1:558786a5d3f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Sep 08 16:34:38 2020 +0000 @@ -0,0 +1,235 @@ +/** 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); + } + +}