Revision 1:558786a5d3f9, committed 2021-06-26

Comitter:

SomeRandomBloke

Date:

Sat Jun 26 12:06:04 2021 +0000

Parent:

0:d19dd72afbb0

Commit message:

Moved to Mbed OS 6

Changed in this revision

--- a/Adafruit_PWMServoDriver.lib	Tue Sep 08 16:34:38 2020 +0000
+++ b/Adafruit_PWMServoDriver.lib	Sat Jun 26 12:06:04 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/SomeRandomBloke/code/Adafruit_PWMServoDriver/#9ff15661ddd6
+https://os.mbed.com/users/SomeRandomBloke/code/Adafruit_PWMServoDriver/#a76057c4c7ac

--- a/main.cpp	Tue Sep 08 16:34:38 2020 +0000
+++ b/main.cpp	Sat Jun 26 12:06:04 2021 +0000
@@ -1,51 +1,104 @@
 /** 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 "MultiChannelRelay.h"
-//#include "PCA9685.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 RIGHT_EAR 0
-#define LEFT_EAR  1
-#define GUN_SERVO 2
-#define EYE_LEDS  3
+
+#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
 
-I2C i2c(I2C_SDA, I2C_SCL);
-
-//PCA9685 pwmServo( I2C_SDA, I2C_SCL, PCA9685::PCA9685_ADDRESS_0, 400000 );
+// 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(0x456,CANStandard);
-
-//I2C i2c(I2C_SDA, I2C_SCL);
-
-MultiChannelRelay relay(&i2c);
+CANMessage msg(NODE_ID,CANStandard);
 
 
 /** CAN **/
@@ -53,50 +106,168 @@
 {
     led =!led;
     can.read(msg);
-//    printf("\033[1;1HCAN TRANSID: 0x%x", msg.id);
+//    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];
     }
-    /*    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;
-        }
-    */
-//    }
+}
+
+// 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();
 
@@ -105,116 +276,104 @@
     //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);
+    // creates a queue with the default size
+//    EventQueue queue;
 
-    // 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);
-    }
+    // Set initial positions of movable parts
+    probeRetract();
+    tailCentre();
+    headRaiseFull();
 
-    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");
+    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);
 
-//    while (true) {
-//        led = !led;
-//        thread_sleep_for(500);
-//    }
 
     can.mode( CAN::Normal );
 //    can.attach(callback( &canRcv ),CAN::RxIrq);
 
     while(1) {
-        led = !led;
+        debug("\033[13;1HLimits Upper %d, Lower %d    \n",headUpperLimit.read(), headLowerLimit.read() );
+//            led = !led;
         if( can.read(msg) > 0 ) {
-//    printf("\033[1;1HCAN TRANSID: 0x%x", msg.id);
+            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];
             }
 
-            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]);
+            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 NODE_RELAY:
-                    // Byte 1 is relay bit pattern
-                    relay.channelCtrl( RcvData[1] & 0x0F );
-                    //printf("Relay %d  \r\n", RcvData[1] & 0x0F );
+                /*
+                                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

--- a/mbed-os.lib	Tue Sep 08 16:34:38 2020 +0000
+++ b/mbed-os.lib	Sat Jun 26 12:06:04 2021 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#aa70f680bb5755e8fea3f93fc6e04d9b3de235bb
+https://github.com/ARMmbed/mbed-os/#b1796dedeb8accde1cbaecf136fab96895e23d81

--- a/mbed_app.json	Tue Sep 08 16:34:38 2020 +0000
+++ b/mbed_app.json	Sat Jun 26 12:06:04 2021 +0000
@@ -1,12 +1,15 @@
 {
-    "requires": ["bare-metal"],
+    "requires": ["bare-metal","events"],
     "target_overrides": {
         "*": {
+            "target.c_lib": "small",
+            "target.printf_lib": "minimal-printf",
+            "platform.minimal-printf-enable-floating-point": false,
+            "platform.stdio-minimal-console-only": true,
             "platform.default-serial-baud-rate": 115200,
             "platform.stdio-baud-rate"         : 115200,
             "platform.stdio-convert-newlines"  : true,
-            "platform.stdio-buffered-serial"   : true
-
+            "platform.stdio-buffered-serial"   : false
         }
     }
 }
\ No newline at end of file