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);
+    }
+
+}