Andrew Lindsay / Mbed OS K9_Head_Controller

K9 Head controlled uses CAN communications, drives ear servos, gun servo and eyes.

Dependencies:   MultiChannelRelay Adafruit_PWMServoDriver

main.cpp@1:558786a5d3f9, 2021-06-26 (annotated)

Committer:
SomeRandomBloke
Date:
Sat Jun 26 12:06:04 2021 +0000
Revision:
1:558786a5d3f9
Parent:
0:d19dd72afbb0 
Moved to Mbed OS 6

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SomeRandomBloke 0:d19dd72afbb0 1 /** K9 Dr Who Robot dog, Head controler.
SomeRandomBloke 0:d19dd72afbb0 2 * Based on NucleoF303k8 CAN Receive node
SomeRandomBloke 1:558786a5d3f9 3 *"requires": ["bare-metal","events"],
SomeRandomBloke 0:d19dd72afbb0 4 *
SomeRandomBloke 0:d19dd72afbb0 5 *
SomeRandomBloke 1:558786a5d3f9 6 * Body controller - Implements Tail, Head nod and Probe.
SomeRandomBloke 0:d19dd72afbb0 7 *
SomeRandomBloke 1:558786a5d3f9 8 * Function Byte 0 Byte 1 Byte 2 Byte 3 Byte 4 Byte 5 Byte 6 Byte 7
SomeRandomBloke 1:558786a5d3f9 9 * Nose Gun Extend 0x01 0x01
SomeRandomBloke 1:558786a5d3f9 10 * Nose Gun Retract 0x01 0x00
SomeRandomBloke 1:558786a5d3f9 11 * Waggle ears 0x02 n
SomeRandomBloke 1:558786a5d3f9 12 * Eye LEDs fade on 0x03 0x01
SomeRandomBloke 1:558786a5d3f9 13 * Eye LEDs off 0x03 0x00
SomeRandomBloke 1:558786a5d3f9 14 * Wag tail left/right 0x04 0x01 n
SomeRandomBloke 1:558786a5d3f9 15 * Raise tail 0x04 0x02
SomeRandomBloke 1:558786a5d3f9 16 * Lower tail 0x04 0x03
SomeRandomBloke 1:558786a5d3f9 17 * Centre tail 0x04 0x00
SomeRandomBloke 1:558786a5d3f9 18 * Move Head up 0x05 0x01
SomeRandomBloke 1:558786a5d3f9 19 * Move Head down 0x05 0x02
SomeRandomBloke 1:558786a5d3f9 20 * Probe Extend 0x06 0x01
SomeRandomBloke 1:558786a5d3f9 21 * Probe Retract 0x06 0x00
SomeRandomBloke 0:d19dd72afbb0 22 *
SomeRandomBloke 1:558786a5d3f9 23 * Relay board 0x81 bit pattern
SomeRandomBloke 1:558786a5d3f9 24 * Motor Control 0x82
SomeRandomBloke 1:558786a5d3f9 25 * PWM Control 0x83
SomeRandomBloke 0:d19dd72afbb0 26 *
SomeRandomBloke 0:d19dd72afbb0 27 */
SomeRandomBloke 0:d19dd72afbb0 28
SomeRandomBloke 0:d19dd72afbb0 29 #include "mbed.h"
SomeRandomBloke 1:558786a5d3f9 30 #include "mbed_events.h"
SomeRandomBloke 1:558786a5d3f9 31 //#include <stdio.h>
SomeRandomBloke 0:d19dd72afbb0 32 #include "Adafruit_PWMServoDriver.h"
SomeRandomBloke 0:d19dd72afbb0 33
SomeRandomBloke 0:d19dd72afbb0 34 // "requires": ["bare-metal"],
SomeRandomBloke 0:d19dd72afbb0 35
SomeRandomBloke 0:d19dd72afbb0 36 #define NODE_ID 0x456
SomeRandomBloke 1:558786a5d3f9 37
SomeRandomBloke 1:558786a5d3f9 38 // Define controller operations
SomeRandomBloke 1:558786a5d3f9 39 #define K9_HEAD_GUN 0x01
SomeRandomBloke 1:558786a5d3f9 40 #define K9_HEAD_EARS 0x02
SomeRandomBloke 1:558786a5d3f9 41 #define K9_HEAD_EYES 0x03
SomeRandomBloke 1:558786a5d3f9 42 #define K9_TAIL 0x04
SomeRandomBloke 1:558786a5d3f9 43 #define K9_HEAD 0x05
SomeRandomBloke 1:558786a5d3f9 44 #define K9_PROBE 0x06
SomeRandomBloke 1:558786a5d3f9 45
SomeRandomBloke 1:558786a5d3f9 46 // Specific I/O functions
SomeRandomBloke 0:d19dd72afbb0 47 #define NODE_RELAY 0x81
SomeRandomBloke 0:d19dd72afbb0 48 #define NODE_MOTOR 0x82
SomeRandomBloke 0:d19dd72afbb0 49 #define NODE_SERVO 0x83
SomeRandomBloke 0:d19dd72afbb0 50
SomeRandomBloke 0:d19dd72afbb0 51
SomeRandomBloke 1:558786a5d3f9 52 // Servo numbers
SomeRandomBloke 1:558786a5d3f9 53 #define RIGHT_EAR 0
SomeRandomBloke 1:558786a5d3f9 54 #define LEFT_EAR 1
SomeRandomBloke 1:558786a5d3f9 55 #define GUN_SERVO 2
SomeRandomBloke 1:558786a5d3f9 56 #define EYE_LEDS 3
SomeRandomBloke 1:558786a5d3f9 57 #define TAIL_HORIZ 4
SomeRandomBloke 1:558786a5d3f9 58 #define TAIL_VERT 5
SomeRandomBloke 1:558786a5d3f9 59
SomeRandomBloke 1:558786a5d3f9 60 // GPIO
SomeRandomBloke 1:558786a5d3f9 61 #define PROBE_RELAY PF_1
SomeRandomBloke 1:558786a5d3f9 62 #define HEAD_UPPER_LIMIT PB_4
SomeRandomBloke 1:558786a5d3f9 63 #define HEAD_LOWER_LIMIT PB_5
SomeRandomBloke 1:558786a5d3f9 64 #define HEAD_MOTOR1 PB_0
SomeRandomBloke 1:558786a5d3f9 65 #define HEAD_MOTOR2 PB_1
SomeRandomBloke 1:558786a5d3f9 66
SomeRandomBloke 1:558786a5d3f9 67 // Servo parameters
SomeRandomBloke 0:d19dd72afbb0 68 #define WAVE_MIN 120
SomeRandomBloke 0:d19dd72afbb0 69 #define WAVE_MAX 450
SomeRandomBloke 0:d19dd72afbb0 70 #define WAVE_FWD 285
SomeRandomBloke 1:558786a5d3f9 71
SomeRandomBloke 1:558786a5d3f9 72 #define EYE_MAX 4095
SomeRandomBloke 1:558786a5d3f9 73 #define EYE_MIN 0
SomeRandomBloke 1:558786a5d3f9 74
SomeRandomBloke 1:558786a5d3f9 75 #define WAG_MIN 220
SomeRandomBloke 1:558786a5d3f9 76 #define WAG_MAX 450
SomeRandomBloke 1:558786a5d3f9 77 #define TAIL_VERT_MIN 220
SomeRandomBloke 1:558786a5d3f9 78 #define TAIL_VERT_MAX 450
SomeRandomBloke 1:558786a5d3f9 79 #define TAIL_CENTRE_HORIZ 285
SomeRandomBloke 1:558786a5d3f9 80 #define TAIL_CENTRE_VERT 320
SomeRandomBloke 0:d19dd72afbb0 81
SomeRandomBloke 0:d19dd72afbb0 82 // Blinking rate in milliseconds
SomeRandomBloke 0:d19dd72afbb0 83 #define BLINKING_RATE 500
SomeRandomBloke 0:d19dd72afbb0 84
SomeRandomBloke 1:558786a5d3f9 85 // Instantiate I2C and servo objects
SomeRandomBloke 1:558786a5d3f9 86 I2C i2c(PB_7, PB_6);
SomeRandomBloke 1:558786a5d3f9 87 //I2C i2c(I2C_SDA, I2C_SCL);
SomeRandomBloke 0:d19dd72afbb0 88 Adafruit_PWMServoDriver pwmServo( &i2c );
SomeRandomBloke 0:d19dd72afbb0 89
SomeRandomBloke 0:d19dd72afbb0 90 void canRcv(void);
SomeRandomBloke 0:d19dd72afbb0 91 char RcvData[8];
SomeRandomBloke 0:d19dd72afbb0 92 DigitalOut led(PB_3);
SomeRandomBloke 1:558786a5d3f9 93 DigitalOut probeRelay( PROBE_RELAY, 0 );
SomeRandomBloke 1:558786a5d3f9 94 DigitalOut headMotor1( HEAD_MOTOR1, 0 );
SomeRandomBloke 1:558786a5d3f9 95 DigitalOut headMotor2( HEAD_MOTOR2, 0 );
SomeRandomBloke 1:558786a5d3f9 96 DigitalIn headUpperLimit( HEAD_UPPER_LIMIT, PullUp );
SomeRandomBloke 1:558786a5d3f9 97 DigitalIn headLowerLimit( HEAD_LOWER_LIMIT, PullUp );
SomeRandomBloke 1:558786a5d3f9 98
SomeRandomBloke 0:d19dd72afbb0 99 //Serial pc(PA_2,PA_3,115200);
SomeRandomBloke 0:d19dd72afbb0 100 CAN can(PA_11, PA_12,500000); // Rx, Tx
SomeRandomBloke 1:558786a5d3f9 101 CANMessage msg(NODE_ID,CANStandard);
SomeRandomBloke 0:d19dd72afbb0 102
SomeRandomBloke 0:d19dd72afbb0 103
SomeRandomBloke 0:d19dd72afbb0 104 /** CAN **/
SomeRandomBloke 0:d19dd72afbb0 105 void canRcv(void)
SomeRandomBloke 0:d19dd72afbb0 106 {
SomeRandomBloke 0:d19dd72afbb0 107 led =!led;
SomeRandomBloke 0:d19dd72afbb0 108 can.read(msg);
SomeRandomBloke 1:558786a5d3f9 109 // debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
SomeRandomBloke 0:d19dd72afbb0 110 // if(msg.id == NODE_ID) {
SomeRandomBloke 0:d19dd72afbb0 111 for(uint8_t i=0; i<8; i++) {
SomeRandomBloke 0:d19dd72afbb0 112 RcvData[i] = msg.data[i];
SomeRandomBloke 0:d19dd72afbb0 113 }
SomeRandomBloke 1:558786a5d3f9 114 }
SomeRandomBloke 1:558786a5d3f9 115
SomeRandomBloke 1:558786a5d3f9 116 // Movement functions
SomeRandomBloke 1:558786a5d3f9 117
SomeRandomBloke 1:558786a5d3f9 118 void probeExtend( void )
SomeRandomBloke 1:558786a5d3f9 119 {
SomeRandomBloke 1:558786a5d3f9 120 debug("\033[12;1HProbe Extend \n");
SomeRandomBloke 1:558786a5d3f9 121 probeRelay = 1;
SomeRandomBloke 1:558786a5d3f9 122 }
SomeRandomBloke 1:558786a5d3f9 123
SomeRandomBloke 1:558786a5d3f9 124 void probeRetract( void )
SomeRandomBloke 1:558786a5d3f9 125 {
SomeRandomBloke 1:558786a5d3f9 126 debug("\033[12;1HProbe Retract \n");
SomeRandomBloke 1:558786a5d3f9 127 probeRelay = 0;
SomeRandomBloke 1:558786a5d3f9 128 }
SomeRandomBloke 1:558786a5d3f9 129
SomeRandomBloke 1:558786a5d3f9 130 void tailCentre()
SomeRandomBloke 1:558786a5d3f9 131 {
SomeRandomBloke 1:558786a5d3f9 132 debug("\033[12;1HTail Centre \n");
SomeRandomBloke 1:558786a5d3f9 133 pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
SomeRandomBloke 1:558786a5d3f9 134 pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
SomeRandomBloke 1:558786a5d3f9 135 }
SomeRandomBloke 1:558786a5d3f9 136
SomeRandomBloke 1:558786a5d3f9 137 void tailWag( uint8_t numWags )
SomeRandomBloke 1:558786a5d3f9 138 {
SomeRandomBloke 1:558786a5d3f9 139 debug("\033[12;HWag Tail %d \n", numWags);
SomeRandomBloke 1:558786a5d3f9 140 pwmServo.setPWM(TAIL_HORIZ, 0, TAIL_CENTRE_HORIZ);
SomeRandomBloke 1:558786a5d3f9 141 pwmServo.setPWM(TAIL_VERT, 0, TAIL_CENTRE_VERT);
SomeRandomBloke 1:558786a5d3f9 142 for( int i=0; i< numWags ; i++ ) {
SomeRandomBloke 1:558786a5d3f9 143 pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MIN);
SomeRandomBloke 1:558786a5d3f9 144 thread_sleep_for(700);
SomeRandomBloke 1:558786a5d3f9 145 pwmServo.setPWM(TAIL_HORIZ, 0, WAG_MAX);
SomeRandomBloke 1:558786a5d3f9 146 thread_sleep_for(700);
SomeRandomBloke 1:558786a5d3f9 147 }
SomeRandomBloke 1:558786a5d3f9 148 tailCentre();
SomeRandomBloke 1:558786a5d3f9 149 }
SomeRandomBloke 1:558786a5d3f9 150
SomeRandomBloke 1:558786a5d3f9 151 void tailRaise()
SomeRandomBloke 1:558786a5d3f9 152 {
SomeRandomBloke 1:558786a5d3f9 153 debug("\033[12;1HTail Raise \n");
SomeRandomBloke 1:558786a5d3f9 154 pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MAX);
SomeRandomBloke 1:558786a5d3f9 155 }
SomeRandomBloke 1:558786a5d3f9 156
SomeRandomBloke 1:558786a5d3f9 157 void tailLower()
SomeRandomBloke 1:558786a5d3f9 158 {
SomeRandomBloke 1:558786a5d3f9 159 debug("\033[12;1HTail Lower \n");
SomeRandomBloke 1:558786a5d3f9 160 pwmServo.setPWM(TAIL_VERT, 0, TAIL_VERT_MIN);
SomeRandomBloke 1:558786a5d3f9 161 }
SomeRandomBloke 1:558786a5d3f9 162
SomeRandomBloke 1:558786a5d3f9 163 void linearMotorOff()
SomeRandomBloke 1:558786a5d3f9 164 {
SomeRandomBloke 1:558786a5d3f9 165 headMotor1.write(0);
SomeRandomBloke 1:558786a5d3f9 166 headMotor2.write(0);
SomeRandomBloke 1:558786a5d3f9 167 }
SomeRandomBloke 1:558786a5d3f9 168
SomeRandomBloke 1:558786a5d3f9 169 void linearMotorUp()
SomeRandomBloke 1:558786a5d3f9 170 {
SomeRandomBloke 1:558786a5d3f9 171 headMotor1.write(0);
SomeRandomBloke 1:558786a5d3f9 172 headMotor2.write(1);
SomeRandomBloke 1:558786a5d3f9 173 }
SomeRandomBloke 1:558786a5d3f9 174
SomeRandomBloke 1:558786a5d3f9 175 void linearMotorDown()
SomeRandomBloke 1:558786a5d3f9 176 {
SomeRandomBloke 1:558786a5d3f9 177 headMotor1.write(1);
SomeRandomBloke 1:558786a5d3f9 178 headMotor2.write(0);
SomeRandomBloke 0:d19dd72afbb0 179 }
SomeRandomBloke 0:d19dd72afbb0 180
SomeRandomBloke 1:558786a5d3f9 181 void headRaiseFull()
SomeRandomBloke 1:558786a5d3f9 182 {
SomeRandomBloke 1:558786a5d3f9 183 debug("\033[12;1HHead Raise Full \n");
SomeRandomBloke 1:558786a5d3f9 184 // Motor to be driven down, check lower limit
SomeRandomBloke 1:558786a5d3f9 185
SomeRandomBloke 1:558786a5d3f9 186 // Check Lower limit switch
SomeRandomBloke 1:558786a5d3f9 187 if( headLowerLimit.read() == 0 )
SomeRandomBloke 1:558786a5d3f9 188 return;
SomeRandomBloke 1:558786a5d3f9 189
SomeRandomBloke 1:558786a5d3f9 190 // Turn on Motor
SomeRandomBloke 1:558786a5d3f9 191 linearMotorDown();
SomeRandomBloke 1:558786a5d3f9 192
SomeRandomBloke 1:558786a5d3f9 193 // Wait for limit
SomeRandomBloke 1:558786a5d3f9 194 while( headLowerLimit.read() != 0 )
SomeRandomBloke 1:558786a5d3f9 195 thread_sleep_for(5);
SomeRandomBloke 1:558786a5d3f9 196
SomeRandomBloke 1:558786a5d3f9 197 // Turn off motor
SomeRandomBloke 1:558786a5d3f9 198 linearMotorOff();
SomeRandomBloke 1:558786a5d3f9 199 }
SomeRandomBloke 1:558786a5d3f9 200
SomeRandomBloke 1:558786a5d3f9 201 void headRaise()
SomeRandomBloke 1:558786a5d3f9 202 {
SomeRandomBloke 1:558786a5d3f9 203 debug("\033[12;1HHead Raise \n");
SomeRandomBloke 1:558786a5d3f9 204 // Motor to be driven down, check lower limit
SomeRandomBloke 1:558786a5d3f9 205
SomeRandomBloke 1:558786a5d3f9 206 // Check Lower limit switch
SomeRandomBloke 1:558786a5d3f9 207 if( headLowerLimit.read() == 0 )
SomeRandomBloke 1:558786a5d3f9 208 return;
SomeRandomBloke 1:558786a5d3f9 209
SomeRandomBloke 1:558786a5d3f9 210 // Turn on Motor
SomeRandomBloke 1:558786a5d3f9 211 linearMotorDown();
SomeRandomBloke 1:558786a5d3f9 212
SomeRandomBloke 1:558786a5d3f9 213 // Wait for lower limit or time reached
SomeRandomBloke 1:558786a5d3f9 214 int timeCount = 200;
SomeRandomBloke 1:558786a5d3f9 215 while( headLowerLimit.read() != 0 && --timeCount > 0 )
SomeRandomBloke 1:558786a5d3f9 216 thread_sleep_for(5);
SomeRandomBloke 1:558786a5d3f9 217
SomeRandomBloke 1:558786a5d3f9 218 // Turn off motor
SomeRandomBloke 1:558786a5d3f9 219 linearMotorOff();
SomeRandomBloke 1:558786a5d3f9 220
SomeRandomBloke 1:558786a5d3f9 221 }
SomeRandomBloke 1:558786a5d3f9 222
SomeRandomBloke 1:558786a5d3f9 223 void headLowerFull()
SomeRandomBloke 1:558786a5d3f9 224 {
SomeRandomBloke 1:558786a5d3f9 225 debug("\033[12;1HHead Lower Full \n");
SomeRandomBloke 1:558786a5d3f9 226 // Motor to be driven up, check upper limit
SomeRandomBloke 1:558786a5d3f9 227
SomeRandomBloke 1:558786a5d3f9 228 // Check Upper limit switch
SomeRandomBloke 1:558786a5d3f9 229 if( headUpperLimit.read() == 0 )
SomeRandomBloke 1:558786a5d3f9 230 return;
SomeRandomBloke 1:558786a5d3f9 231
SomeRandomBloke 1:558786a5d3f9 232 // Turn on Motor
SomeRandomBloke 1:558786a5d3f9 233 linearMotorUp();
SomeRandomBloke 1:558786a5d3f9 234
SomeRandomBloke 1:558786a5d3f9 235 // Wait for limit
SomeRandomBloke 1:558786a5d3f9 236 while( headUpperLimit.read() != 0 )
SomeRandomBloke 1:558786a5d3f9 237 thread_sleep_for(5);
SomeRandomBloke 1:558786a5d3f9 238
SomeRandomBloke 1:558786a5d3f9 239 // Turn off motor
SomeRandomBloke 1:558786a5d3f9 240 linearMotorOff();
SomeRandomBloke 1:558786a5d3f9 241 }
SomeRandomBloke 1:558786a5d3f9 242
SomeRandomBloke 1:558786a5d3f9 243 void headLower()
SomeRandomBloke 1:558786a5d3f9 244 {
SomeRandomBloke 1:558786a5d3f9 245 debug("\033[12;1HHead Lower \n");
SomeRandomBloke 1:558786a5d3f9 246 // Motor to be driven up, check upper limit
SomeRandomBloke 1:558786a5d3f9 247
SomeRandomBloke 1:558786a5d3f9 248 // Check Lower limit switch
SomeRandomBloke 1:558786a5d3f9 249 if( headLowerLimit.read() == 0 )
SomeRandomBloke 1:558786a5d3f9 250 return;
SomeRandomBloke 1:558786a5d3f9 251
SomeRandomBloke 1:558786a5d3f9 252 // Turn on Motor
SomeRandomBloke 1:558786a5d3f9 253 linearMotorDown();
SomeRandomBloke 1:558786a5d3f9 254
SomeRandomBloke 1:558786a5d3f9 255 // Wait for upper limit
SomeRandomBloke 1:558786a5d3f9 256 while( headLowerLimit.read() != 0 )
SomeRandomBloke 1:558786a5d3f9 257 thread_sleep_for(5);
SomeRandomBloke 1:558786a5d3f9 258
SomeRandomBloke 1:558786a5d3f9 259 // Turn off motor
SomeRandomBloke 1:558786a5d3f9 260 linearMotorOff();
SomeRandomBloke 1:558786a5d3f9 261 }
SomeRandomBloke 1:558786a5d3f9 262
SomeRandomBloke 1:558786a5d3f9 263
SomeRandomBloke 0:d19dd72afbb0 264
SomeRandomBloke 0:d19dd72afbb0 265 int main()
SomeRandomBloke 0:d19dd72afbb0 266 {
SomeRandomBloke 0:d19dd72afbb0 267 // Setup hardware
SomeRandomBloke 0:d19dd72afbb0 268
SomeRandomBloke 1:558786a5d3f9 269 headMotor1 = 0;
SomeRandomBloke 1:558786a5d3f9 270 headMotor2 = 0;
SomeRandomBloke 0:d19dd72afbb0 271 //PCA9685::PCA9685_status_t status = pwmServo.PCA9685_SoftReset();
SomeRandomBloke 0:d19dd72afbb0 272 pwmServo.begin();
SomeRandomBloke 0:d19dd72afbb0 273
SomeRandomBloke 0:d19dd72afbb0 274 // Configure the PWM frequency and wake up the device
SomeRandomBloke 0:d19dd72afbb0 275 //status = pwmServo.PCA9685_SetPWM_Freq( 1000 ); // PWM frequency: 1kHz
SomeRandomBloke 0:d19dd72afbb0 276 //status = pwmServo.PCA9685_SetMode( PCA9685::MODE1_SLEEP_DISABLED );
SomeRandomBloke 0:d19dd72afbb0 277 pwmServo.setPWMFreq(50); // 50Hz
SomeRandomBloke 0:d19dd72afbb0 278
SomeRandomBloke 1:558786a5d3f9 279 // creates a queue with the default size
SomeRandomBloke 1:558786a5d3f9 280 // EventQueue queue;
SomeRandomBloke 0:d19dd72afbb0 281
SomeRandomBloke 1:558786a5d3f9 282 // Set initial positions of movable parts
SomeRandomBloke 1:558786a5d3f9 283 probeRetract();
SomeRandomBloke 1:558786a5d3f9 284 tailCentre();
SomeRandomBloke 1:558786a5d3f9 285 headRaiseFull();
SomeRandomBloke 0:d19dd72afbb0 286
SomeRandomBloke 0:d19dd72afbb0 287
SomeRandomBloke 1:558786a5d3f9 288 debug("\033[2J\033[1;1HSTART K9 Body Controller, ");
SomeRandomBloke 1:558786a5d3f9 289 debug("mbed-os: %d.%d.%d\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);
SomeRandomBloke 0:d19dd72afbb0 290
SomeRandomBloke 0:d19dd72afbb0 291 // Initialise the digital pin LED1 as an output
SomeRandomBloke 0:d19dd72afbb0 292 DigitalOut led(LED1);
SomeRandomBloke 0:d19dd72afbb0 293
SomeRandomBloke 0:d19dd72afbb0 294
SomeRandomBloke 0:d19dd72afbb0 295 can.mode( CAN::Normal );
SomeRandomBloke 0:d19dd72afbb0 296 // can.attach(callback( &canRcv ),CAN::RxIrq);
SomeRandomBloke 0:d19dd72afbb0 297
SomeRandomBloke 0:d19dd72afbb0 298 while(1) {
SomeRandomBloke 1:558786a5d3f9 299 debug("\033[13;1HLimits Upper %d, Lower %d \n",headUpperLimit.read(), headLowerLimit.read() );
SomeRandomBloke 1:558786a5d3f9 300 // led = !led;
SomeRandomBloke 0:d19dd72afbb0 301 if( can.read(msg) > 0 ) {
SomeRandomBloke 1:558786a5d3f9 302 led = !led;
SomeRandomBloke 1:558786a5d3f9 303 // debug("\033[1;1HCAN TRANSID: 0x%x", msg.id);
SomeRandomBloke 0:d19dd72afbb0 304 // if(msg.id == NODE_ID) {
SomeRandomBloke 0:d19dd72afbb0 305 for(uint8_t i=0; i<8; i++) {
SomeRandomBloke 0:d19dd72afbb0 306 RcvData[i] = msg.data[i];
SomeRandomBloke 0:d19dd72afbb0 307 }
SomeRandomBloke 0:d19dd72afbb0 308
SomeRandomBloke 1:558786a5d3f9 309 debug("\033[2;1HCAN DATA[0]: 0x%x", RcvData[0]);
SomeRandomBloke 1:558786a5d3f9 310 debug("\033[3;1HCAN DATA[1]: 0x%x", RcvData[1]);
SomeRandomBloke 1:558786a5d3f9 311 debug("\033[4;1HCAN DATA[2]: 0x%x", RcvData[2]);
SomeRandomBloke 1:558786a5d3f9 312 debug("\033[5;1HCAN DATA[3]: 0x%x", RcvData[3]);
SomeRandomBloke 1:558786a5d3f9 313 debug("\033[6;1HCAN DATA[4]: 0x%x", RcvData[4]);
SomeRandomBloke 1:558786a5d3f9 314 debug("\033[7;1HCAN DATA[5]: 0x%x", RcvData[5]);
SomeRandomBloke 1:558786a5d3f9 315 debug("\033[8;1HCAN DATA[6]: 0x%x", RcvData[6]);
SomeRandomBloke 1:558786a5d3f9 316 debug("\033[9;1HCAN DATA[7]: 0x%x", RcvData[7]);
SomeRandomBloke 0:d19dd72afbb0 317
SomeRandomBloke 0:d19dd72afbb0 318 // Byte 0 is node type
SomeRandomBloke 0:d19dd72afbb0 319 switch( RcvData[0] ) {
SomeRandomBloke 1:558786a5d3f9 320 /*
SomeRandomBloke 1:558786a5d3f9 321 case K9_HEAD_GUN: // Move Gun
SomeRandomBloke 1:558786a5d3f9 322 if( RcvData[1] == 0x01 ) // Extend gun
SomeRandomBloke 1:558786a5d3f9 323 gunExtend();
SomeRandomBloke 1:558786a5d3f9 324 else if( RcvData[1] == 0x00 ) // Retract gun
SomeRandomBloke 1:558786a5d3f9 325 gunRetract();
SomeRandomBloke 1:558786a5d3f9 326 break;
SomeRandomBloke 1:558786a5d3f9 327
SomeRandomBloke 1:558786a5d3f9 328 case K9_HEAD_EARS: // Move ears
SomeRandomBloke 1:558786a5d3f9 329 if( RcvData[1] > 0x00 ) // Ear scan
SomeRandomBloke 1:558786a5d3f9 330 earScan( RcvData[1] );
SomeRandomBloke 1:558786a5d3f9 331 break;
SomeRandomBloke 1:558786a5d3f9 332
SomeRandomBloke 1:558786a5d3f9 333 case K9_HEAD_EYES: // Flash/fade eyes
SomeRandomBloke 1:558786a5d3f9 334 if( RcvData[1] > 0x00 ) // Fade eyes
SomeRandomBloke 1:558786a5d3f9 335 eyesStartFade();
SomeRandomBloke 1:558786a5d3f9 336 else
SomeRandomBloke 1:558786a5d3f9 337 eyesStopFade();
SomeRandomBloke 1:558786a5d3f9 338
SomeRandomBloke 1:558786a5d3f9 339 break;
SomeRandomBloke 1:558786a5d3f9 340 */
SomeRandomBloke 1:558786a5d3f9 341 case K9_TAIL: // Move Tail
SomeRandomBloke 1:558786a5d3f9 342 if( RcvData[1] == 0x01 ) // Wag Tail
SomeRandomBloke 1:558786a5d3f9 343 tailWag(RcvData[2]);
SomeRandomBloke 1:558786a5d3f9 344 else if( RcvData[1] == 0x02 ) // Raise Tail
SomeRandomBloke 1:558786a5d3f9 345 tailRaise();
SomeRandomBloke 1:558786a5d3f9 346 else if( RcvData[1] == 0x03 ) // Lower Tail
SomeRandomBloke 1:558786a5d3f9 347 tailLower();
SomeRandomBloke 1:558786a5d3f9 348 else
SomeRandomBloke 1:558786a5d3f9 349 tailCentre();
SomeRandomBloke 0:d19dd72afbb0 350 break;
SomeRandomBloke 1:558786a5d3f9 351
SomeRandomBloke 1:558786a5d3f9 352 case K9_HEAD: // Move Head up/down
SomeRandomBloke 1:558786a5d3f9 353 if( RcvData[1] == 0x01 ) //Raise Head
SomeRandomBloke 1:558786a5d3f9 354 headRaiseFull();
SomeRandomBloke 1:558786a5d3f9 355 else if( RcvData[1] == 0x02 ) // Lower Head
SomeRandomBloke 1:558786a5d3f9 356 headLowerFull();
SomeRandomBloke 1:558786a5d3f9 357 break;
SomeRandomBloke 1:558786a5d3f9 358
SomeRandomBloke 1:558786a5d3f9 359 case K9_PROBE: // Move Probe
SomeRandomBloke 1:558786a5d3f9 360 if( RcvData[1] == 0x01 ) // Extend Probe
SomeRandomBloke 1:558786a5d3f9 361 probeExtend();
SomeRandomBloke 1:558786a5d3f9 362 else if( RcvData[1] == 0x00 ) // Retract Probe
SomeRandomBloke 1:558786a5d3f9 363 probeRetract();
SomeRandomBloke 1:558786a5d3f9 364 break;
SomeRandomBloke 1:558786a5d3f9 365 /*
SomeRandomBloke 1:558786a5d3f9 366 case NODE_RELAY:
SomeRandomBloke 1:558786a5d3f9 367 // Byte 1 is relay bit pattern
SomeRandomBloke 1:558786a5d3f9 368 relay.channelCtrl( RcvData[1] & 0x0F );
SomeRandomBloke 1:558786a5d3f9 369 //debug("Relay %d \r\n", RcvData[1] & 0x0F );
SomeRandomBloke 1:558786a5d3f9 370 break;
SomeRandomBloke 1:558786a5d3f9 371
SomeRandomBloke 0:d19dd72afbb0 372 case NODE_MOTOR:
SomeRandomBloke 0:d19dd72afbb0 373 // Bytes 1/2 L/H signed int Right direction/speed
SomeRandomBloke 0:d19dd72afbb0 374 // Bytes 3/4 L/H signed int Left direction/speed
SomeRandomBloke 0:d19dd72afbb0 375 break;
SomeRandomBloke 1:558786a5d3f9 376 */
SomeRandomBloke 0:d19dd72afbb0 377 case NODE_SERVO:
SomeRandomBloke 0:d19dd72afbb0 378 // Byte 1 servo number
SomeRandomBloke 0:d19dd72afbb0 379 // Byte 2/3 L/H value
SomeRandomBloke 0:d19dd72afbb0 380 uint16_t servoVal = (RcvData[3] << 8) | RcvData[2];
SomeRandomBloke 0:d19dd72afbb0 381 pwmServo.setPWM(RcvData[1] & 0x0F, 0, servoVal );
SomeRandomBloke 0:d19dd72afbb0 382 break;
SomeRandomBloke 0:d19dd72afbb0 383
SomeRandomBloke 0:d19dd72afbb0 384 // default: // Reset data
SomeRandomBloke 0:d19dd72afbb0 385 // RcvData[0] = 0;
SomeRandomBloke 0:d19dd72afbb0 386 // break;
SomeRandomBloke 0:d19dd72afbb0 387
SomeRandomBloke 0:d19dd72afbb0 388 }
SomeRandomBloke 0:d19dd72afbb0 389 RcvData[0] = 0;
SomeRandomBloke 0:d19dd72afbb0 390 }
SomeRandomBloke 0:d19dd72afbb0 391 thread_sleep_for(100);
SomeRandomBloke 0:d19dd72afbb0 392 }
SomeRandomBloke 0:d19dd72afbb0 393
SomeRandomBloke 0:d19dd72afbb0 394 }