Feng Hong / Mbed OS Nucleo_rtos_basic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

payload.cpp@21:5c6b3657c3cb, 2020-02-22 (annotated)

Committer:
hi1000
Date:
Sat Feb 22 04:53:57 2020 +0000
Revision:
21:5c6b3657c3cb
Parent:
20:ec9d4f6a16ac 
Latest codes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hi1000 7:e0c7e624c5fa 1 #include "mbed.h"
hi1000 18:e6ed582f7022 2 #include "yoda2.h"
hi1000 19:0356e54240cc 3 #if 1
hi1000 8:6105ffbaf237 4 extern unsigned char rx_buffer[8], tx_buffer[8];
hi1000 7:e0c7e624c5fa 5 extern unsigned char rx_length, tx_length;
hi1000 9:486f65124378 6 extern uint8_t can_tx_data[8];
hi1000 9:486f65124378 7 extern uint8_t can_rx_data[8];
hi1000 17:faa4d4976d22 8 extern int current_weight;
hi1000 19:0356e54240cc 9 extern bool can_register_success;
hi1000 16:f4277e9b8612 10
hi1000 16:f4277e9b8612 11 typedef struct can_id_d {
hi1000 16:f4277e9b8612 12 int dest_id;
hi1000 16:f4277e9b8612 13 int source_id;
hi1000 16:f4277e9b8612 14 int message_id;
hi1000 16:f4277e9b8612 15 };
hi1000 16:f4277e9b8612 16
hi1000 16:f4277e9b8612 17 can_id_d can_id_s;
hi1000 16:f4277e9b8612 18 data_field_d can_rxdata_frame;
hi1000 17:faa4d4976d22 19 data_field_d can_txdata_frame;
hi1000 9:486f65124378 20 CANMessage tx_message;
hi1000 9:486f65124378 21 extern MemoryPool<CANMessage, 16> can_mpool;
hi1000 9:486f65124378 22 extern Queue<CANMessage, 16> can_queue;
hi1000 17:faa4d4976d22 23 extern void can_sendData(int can_id, uint8_t *tx_data, int length);
hi1000 18:e6ed582f7022 24 extern Device_Type_d device_type_v;
hi1000 18:e6ed582f7022 25 extern int device_address; // last 8 bits of address
hi1000 18:e6ed582f7022 26 extern int device_type; // first 3 bits of adddress
hi1000 18:e6ed582f7022 27
hi1000 20:ec9d4f6a16ac 28 DigitalOut motor1_en(PB_11);
hi1000 20:ec9d4f6a16ac 29 DigitalOut motor1_dir(PB_2);
hi1000 20:ec9d4f6a16ac 30 DigitalOut motor1_pul(PB_1);
hi1000 20:ec9d4f6a16ac 31
hi1000 20:ec9d4f6a16ac 32 DigitalOut motor2_en(PB_15);
hi1000 20:ec9d4f6a16ac 33 DigitalOut motor2_dir(PB_14);
hi1000 20:ec9d4f6a16ac 34 DigitalOut motor2_pul(PB_13);
hi1000 20:ec9d4f6a16ac 35
hi1000 20:ec9d4f6a16ac 36 DigitalOut motor3_en(PC_6);
hi1000 20:ec9d4f6a16ac 37 DigitalOut motor3_dir(PB_6);
hi1000 20:ec9d4f6a16ac 38 DigitalOut motor3_pul(PB_12);
hi1000 20:ec9d4f6a16ac 39
hi1000 20:ec9d4f6a16ac 40 int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome);
hi1000 20:ec9d4f6a16ac 41 int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome);
hi1000 20:ec9d4f6a16ac 42 int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome);
hi1000 20:ec9d4f6a16ac 43
hi1000 20:ec9d4f6a16ac 44 int delay_us (int us)
hi1000 20:ec9d4f6a16ac 45 {
hi1000 20:ec9d4f6a16ac 46 int ret;
hi1000 20:ec9d4f6a16ac 47 int i, j;
hi1000 20:ec9d4f6a16ac 48 for (i =0; i<us; i++)
hi1000 20:ec9d4f6a16ac 49 {
hi1000 20:ec9d4f6a16ac 50 for (j = 0; j <10000; j++)
hi1000 20:ec9d4f6a16ac 51 {
hi1000 20:ec9d4f6a16ac 52 ret++;
hi1000 20:ec9d4f6a16ac 53 }
hi1000 20:ec9d4f6a16ac 54 }
hi1000 20:ec9d4f6a16ac 55 return ret;
hi1000 20:ec9d4f6a16ac 56 }
hi1000 20:ec9d4f6a16ac 57 int moveMotor(int motornumber, int distance_mm, bool direction, bool freemove, bool gobackhome)
hi1000 20:ec9d4f6a16ac 58 {
hi1000 20:ec9d4f6a16ac 59 int ret = 0;
hi1000 20:ec9d4f6a16ac 60 switch(motornumber) // 0: cuptrack0 1:cuptrack1 2:cuptrack2
hi1000 20:ec9d4f6a16ac 61 {
hi1000 20:ec9d4f6a16ac 62 case 1:
hi1000 20:ec9d4f6a16ac 63 ret = moveMotor1(distance_mm, direction, freemove, gobackhome);
hi1000 20:ec9d4f6a16ac 64 case 2:
hi1000 20:ec9d4f6a16ac 65 ret = moveMotor2(distance_mm, direction, freemove, gobackhome);
hi1000 20:ec9d4f6a16ac 66 case 3:
hi1000 20:ec9d4f6a16ac 67 ret = moveMotor3(distance_mm, direction, freemove, gobackhome);
hi1000 20:ec9d4f6a16ac 68 default:
hi1000 20:ec9d4f6a16ac 69 return -1;
hi1000 20:ec9d4f6a16ac 70 }
hi1000 20:ec9d4f6a16ac 71 return ret;
hi1000 20:ec9d4f6a16ac 72 }
hi1000 20:ec9d4f6a16ac 73 int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome)
hi1000 20:ec9d4f6a16ac 74 {
hi1000 20:ec9d4f6a16ac 75 int loop;
hi1000 20:ec9d4f6a16ac 76 float mm_per_pulse = 143;
hi1000 20:ec9d4f6a16ac 77 if (freemove)
hi1000 20:ec9d4f6a16ac 78 {
hi1000 20:ec9d4f6a16ac 79 //disable motor
hi1000 20:ec9d4f6a16ac 80 motor1_en = 1;
hi1000 20:ec9d4f6a16ac 81 printf("freemove\r\n");
hi1000 20:ec9d4f6a16ac 82 return 0;
hi1000 20:ec9d4f6a16ac 83 }
hi1000 20:ec9d4f6a16ac 84 motor1_en = 0;
hi1000 20:ec9d4f6a16ac 85 if (gobackhome)
hi1000 20:ec9d4f6a16ac 86 {
hi1000 20:ec9d4f6a16ac 87 //move cup to home
hi1000 20:ec9d4f6a16ac 88 printf("go back home\r\n");
hi1000 20:ec9d4f6a16ac 89 return 0;
hi1000 20:ec9d4f6a16ac 90 }
hi1000 20:ec9d4f6a16ac 91 else
hi1000 20:ec9d4f6a16ac 92 {
hi1000 20:ec9d4f6a16ac 93 printf("motor1 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
hi1000 20:ec9d4f6a16ac 94 if (direction) // true: move forward false: move backward
hi1000 20:ec9d4f6a16ac 95 {
hi1000 20:ec9d4f6a16ac 96 //move forward
hi1000 20:ec9d4f6a16ac 97 motor1_dir = 1;
hi1000 20:ec9d4f6a16ac 98 delay_us(400);
hi1000 20:ec9d4f6a16ac 99 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 100 {
hi1000 20:ec9d4f6a16ac 101 motor1_pul = 0;
hi1000 20:ec9d4f6a16ac 102 delay_us(400);
hi1000 20:ec9d4f6a16ac 103 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 104 motor1_pul = 1;
hi1000 20:ec9d4f6a16ac 105 delay_us(400);
hi1000 20:ec9d4f6a16ac 106 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 107 }
hi1000 20:ec9d4f6a16ac 108 }
hi1000 20:ec9d4f6a16ac 109 else
hi1000 20:ec9d4f6a16ac 110 {
hi1000 20:ec9d4f6a16ac 111 //move backward
hi1000 20:ec9d4f6a16ac 112 motor1_dir = 0;
hi1000 20:ec9d4f6a16ac 113 delay_us(400);
hi1000 20:ec9d4f6a16ac 114 // wait(0.00001); //5ms
hi1000 20:ec9d4f6a16ac 115 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 116 {
hi1000 20:ec9d4f6a16ac 117 motor1_pul = 0;
hi1000 20:ec9d4f6a16ac 118 delay_us(400);
hi1000 20:ec9d4f6a16ac 119 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 120 motor1_pul = 1;
hi1000 20:ec9d4f6a16ac 121 delay_us(400);
hi1000 20:ec9d4f6a16ac 122 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 123 }
hi1000 20:ec9d4f6a16ac 124 }
hi1000 20:ec9d4f6a16ac 125 }
hi1000 20:ec9d4f6a16ac 126 }
hi1000 20:ec9d4f6a16ac 127 int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome)
hi1000 20:ec9d4f6a16ac 128 {
hi1000 20:ec9d4f6a16ac 129 int loop;
hi1000 20:ec9d4f6a16ac 130 float mm_per_pulse = 143;
hi1000 20:ec9d4f6a16ac 131 if (freemove)
hi1000 20:ec9d4f6a16ac 132 {
hi1000 20:ec9d4f6a16ac 133 //disable motor
hi1000 20:ec9d4f6a16ac 134 motor2_en = 1;
hi1000 20:ec9d4f6a16ac 135 printf("freemove\r\n");
hi1000 20:ec9d4f6a16ac 136 return 0;
hi1000 20:ec9d4f6a16ac 137 }
hi1000 20:ec9d4f6a16ac 138 motor2_en = 0;
hi1000 20:ec9d4f6a16ac 139 if (gobackhome)
hi1000 20:ec9d4f6a16ac 140 {
hi1000 20:ec9d4f6a16ac 141 //move cup to home
hi1000 20:ec9d4f6a16ac 142 printf("go back home\r\n");
hi1000 20:ec9d4f6a16ac 143 return 0;
hi1000 20:ec9d4f6a16ac 144 }
hi1000 20:ec9d4f6a16ac 145 else
hi1000 20:ec9d4f6a16ac 146 {
hi1000 20:ec9d4f6a16ac 147 printf("motor2 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
hi1000 20:ec9d4f6a16ac 148 if (direction) // true: move forward false: move backward
hi1000 20:ec9d4f6a16ac 149 {
hi1000 20:ec9d4f6a16ac 150 //move forward
hi1000 20:ec9d4f6a16ac 151 motor2_dir = 1;
hi1000 20:ec9d4f6a16ac 152 delay_us(400);
hi1000 20:ec9d4f6a16ac 153 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 154 {
hi1000 20:ec9d4f6a16ac 155 motor2_pul = 0;
hi1000 20:ec9d4f6a16ac 156 delay_us(400);
hi1000 20:ec9d4f6a16ac 157 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 158 motor2_pul = 1;
hi1000 20:ec9d4f6a16ac 159 delay_us(400);
hi1000 20:ec9d4f6a16ac 160 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 161 }
hi1000 20:ec9d4f6a16ac 162 }
hi1000 20:ec9d4f6a16ac 163 else
hi1000 20:ec9d4f6a16ac 164 {
hi1000 20:ec9d4f6a16ac 165 //move backward
hi1000 20:ec9d4f6a16ac 166 motor2_dir = 0;
hi1000 20:ec9d4f6a16ac 167 delay_us(400);
hi1000 20:ec9d4f6a16ac 168 // wait(0.00001); //5ms
hi1000 20:ec9d4f6a16ac 169 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 170 {
hi1000 20:ec9d4f6a16ac 171 motor2_pul = 0;
hi1000 20:ec9d4f6a16ac 172 delay_us(400);
hi1000 20:ec9d4f6a16ac 173 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 174 motor2_pul = 1;
hi1000 20:ec9d4f6a16ac 175 delay_us(400);
hi1000 20:ec9d4f6a16ac 176 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 177 }
hi1000 20:ec9d4f6a16ac 178 }
hi1000 20:ec9d4f6a16ac 179 }
hi1000 20:ec9d4f6a16ac 180 }
hi1000 20:ec9d4f6a16ac 181 int moveMotor3Until(bool direction, DigitalIn sensorpin)
hi1000 20:ec9d4f6a16ac 182 {
hi1000 20:ec9d4f6a16ac 183 int distance = 0;
hi1000 20:ec9d4f6a16ac 184 int mm_per_pulse = 143;
hi1000 20:ec9d4f6a16ac 185 if (direction)
hi1000 20:ec9d4f6a16ac 186 motor3_dir = 1;
hi1000 20:ec9d4f6a16ac 187 else
hi1000 20:ec9d4f6a16ac 188 motor3_dir = 0;
hi1000 20:ec9d4f6a16ac 189 while(sensorpin.read() == 0)
hi1000 20:ec9d4f6a16ac 190 {
hi1000 20:ec9d4f6a16ac 191 // printf("sensorpin %d\r\n", sensorpin.read());
hi1000 20:ec9d4f6a16ac 192 distance++;
hi1000 20:ec9d4f6a16ac 193 motor3_pul = 0;
hi1000 20:ec9d4f6a16ac 194 delay_us(400);
hi1000 20:ec9d4f6a16ac 195 //wait(0.00003);
hi1000 20:ec9d4f6a16ac 196 motor3_pul = 1;
hi1000 20:ec9d4f6a16ac 197 delay_us(400);
hi1000 20:ec9d4f6a16ac 198 //wait(0.00003);
hi1000 20:ec9d4f6a16ac 199 }
hi1000 20:ec9d4f6a16ac 200 return (distance/mm_per_pulse);
hi1000 20:ec9d4f6a16ac 201 }
hi1000 20:ec9d4f6a16ac 202 int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome)
hi1000 20:ec9d4f6a16ac 203 {
hi1000 20:ec9d4f6a16ac 204 int loop;
hi1000 20:ec9d4f6a16ac 205 float mm_per_pulse = 143;
hi1000 20:ec9d4f6a16ac 206 if (freemove)
hi1000 20:ec9d4f6a16ac 207 {
hi1000 20:ec9d4f6a16ac 208 //disable motor
hi1000 20:ec9d4f6a16ac 209 motor3_en = 1;
hi1000 20:ec9d4f6a16ac 210 printf("freemove\r\n");
hi1000 20:ec9d4f6a16ac 211 return 0;
hi1000 20:ec9d4f6a16ac 212 }
hi1000 20:ec9d4f6a16ac 213 motor3_en = 0;
hi1000 20:ec9d4f6a16ac 214 if (gobackhome)
hi1000 20:ec9d4f6a16ac 215 {
hi1000 20:ec9d4f6a16ac 216 //move cup to home
hi1000 20:ec9d4f6a16ac 217 printf("go back home\r\n");
hi1000 20:ec9d4f6a16ac 218 return 0;
hi1000 20:ec9d4f6a16ac 219 }
hi1000 20:ec9d4f6a16ac 220 else
hi1000 20:ec9d4f6a16ac 221 {
hi1000 20:ec9d4f6a16ac 222 printf("motor3 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
hi1000 20:ec9d4f6a16ac 223 if (direction) // true: move forward false: move backward
hi1000 20:ec9d4f6a16ac 224 {
hi1000 20:ec9d4f6a16ac 225 //move forward
hi1000 20:ec9d4f6a16ac 226 motor3_dir = 1;
hi1000 20:ec9d4f6a16ac 227 delay_us(400);
hi1000 20:ec9d4f6a16ac 228 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 229 {
hi1000 20:ec9d4f6a16ac 230 motor3_pul = 0;
hi1000 20:ec9d4f6a16ac 231 delay_us(400);
hi1000 20:ec9d4f6a16ac 232 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 233 motor3_pul = 1;
hi1000 20:ec9d4f6a16ac 234 delay_us(400);
hi1000 20:ec9d4f6a16ac 235 // wait(0.00004);
hi1000 20:ec9d4f6a16ac 236 }
hi1000 20:ec9d4f6a16ac 237 }
hi1000 20:ec9d4f6a16ac 238 else
hi1000 20:ec9d4f6a16ac 239 {
hi1000 20:ec9d4f6a16ac 240 //move backward
hi1000 20:ec9d4f6a16ac 241 motor3_dir = 0;
hi1000 20:ec9d4f6a16ac 242 delay_us(400);
hi1000 20:ec9d4f6a16ac 243 // wait(0.00001); //5ms
hi1000 20:ec9d4f6a16ac 244 for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
hi1000 20:ec9d4f6a16ac 245 {
hi1000 20:ec9d4f6a16ac 246 motor3_pul = 0;
hi1000 20:ec9d4f6a16ac 247 delay_us(400);
hi1000 20:ec9d4f6a16ac 248 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 249 motor3_pul = 1;
hi1000 20:ec9d4f6a16ac 250 delay_us(400);
hi1000 20:ec9d4f6a16ac 251 // wait(0.00003);
hi1000 20:ec9d4f6a16ac 252 }
hi1000 20:ec9d4f6a16ac 253 }
hi1000 20:ec9d4f6a16ac 254 }
hi1000 20:ec9d4f6a16ac 255 }
hi1000 19:0356e54240cc 256
hi1000 18:e6ed582f7022 257 void handleCupTrackCommand(data_field_d data_pack)
hi1000 18:e6ed582f7022 258 {
hi1000 18:e6ed582f7022 259 }
hi1000 18:e6ed582f7022 260 void handleJamTrackCommand(data_field_d data_pack)
hi1000 18:e6ed582f7022 261 {
hi1000 18:e6ed582f7022 262 }
hi1000 18:e6ed582f7022 263 void handleTeaTrackCommand(data_field_d data_pack)
hi1000 18:e6ed582f7022 264 {
hi1000 18:e6ed582f7022 265 }
hi1000 18:e6ed582f7022 266 void handleJamCommand(data_field_d data_pack)
hi1000 18:e6ed582f7022 267 {
hi1000 18:e6ed582f7022 268 }
hi1000 18:e6ed582f7022 269 void handleTeaCommand(data_field_d data_pack)
hi1000 18:e6ed582f7022 270 {
hi1000 18:e6ed582f7022 271 int can_id;
hi1000 19:0356e54240cc 272 int init_weight;
hi1000 21:5c6b3657c3cb 273 int weight;
hi1000 19:0356e54240cc 274 int loop;
hi1000 18:e6ed582f7022 275 switch (data_pack.cmd)
hi1000 18:e6ed582f7022 276 {
hi1000 18:e6ed582f7022 277 case COMMAND_INIT:
hi1000 18:e6ed582f7022 278 can_txdata_frame.cmd = COMMAND_INIT;
hi1000 18:e6ed582f7022 279 can_txdata_frame.value1 = current_weight;
hi1000 18:e6ed582f7022 280 can_txdata_frame.value2 = 0;
hi1000 18:e6ed582f7022 281 can_txdata_frame.value3 = 0;
hi1000 19:0356e54240cc 282 memcpy(can_tx_data, (unsigned char *)&can_txdata_frame, sizeof(can_tx_data));
hi1000 19:0356e54240cc 283 // printf("cmd=0x%08x value1=0x%08x size=%d %d\r\n", can_txdata_frame.cmd, can_txdata_frame.value1, sizeof(can_tx_data), sizeof(can_txdata_frame));
hi1000 19:0356e54240cc 284 // printf("data 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x \r\n", can_tx_data[0], can_tx_data[1], can_tx_data[2], can_tx_data[3], can_tx_data[4], can_tx_data[5], can_tx_data[6], can_tx_data[7]);
hi1000 18:e6ed582f7022 285 can_id = (can_id_s.source_id << 18) | (can_id_s.dest_id << 7) | can_id_s.message_id | 0x80000000;
hi1000 18:e6ed582f7022 286 can_sendData(can_id, can_tx_data, 8);
hi1000 18:e6ed582f7022 287 break;
hi1000 19:0356e54240cc 288 case COMMAND_PLUS:
hi1000 19:0356e54240cc 289 init_weight = current_weight;
hi1000 21:5c6b3657c3cb 290 weight = data_pack.value1;
hi1000 21:5c6b3657c3cb 291 printf("init_weight:%dg needed_weight:%dg\r\n", init_weight, weight);
hi1000 21:5c6b3657c3cb 292 while ((current_weight - init_weight) <= weight)
hi1000 19:0356e54240cc 293 {
hi1000 19:0356e54240cc 294 printf("still need to plus weight %dg \r\n", (current_weight - init_weight));
hi1000 19:0356e54240cc 295 }
hi1000 19:0356e54240cc 296 break;
hi1000 18:e6ed582f7022 297 default:
hi1000 18:e6ed582f7022 298 break;
hi1000 18:e6ed582f7022 299 }
hi1000 18:e6ed582f7022 300 }
hi1000 18:e6ed582f7022 301 void handleShakerCommand(data_field_d command)
hi1000 18:e6ed582f7022 302 {
hi1000 18:e6ed582f7022 303 }
hi1000 19:0356e54240cc 304 void handleIceMakerCommand(data_field_d command)
hi1000 19:0356e54240cc 305 {
hi1000 19:0356e54240cc 306 }
hi1000 7:e0c7e624c5fa 307 void analyzePayload()
hi1000 7:e0c7e624c5fa 308 {
hi1000 17:faa4d4976d22 309 int can_id;
hi1000 9:486f65124378 310 int loop;
hi1000 8:6105ffbaf237 311
hi1000 8:6105ffbaf237 312 printf("analyzePayload thread\r\n");
hi1000 8:6105ffbaf237 313 while (true) {
hi1000 8:6105ffbaf237 314 osEvent evt = can_queue.get();
hi1000 8:6105ffbaf237 315 if (evt.status == osEventMessage) {
hi1000 8:6105ffbaf237 316 CANMessage *message = (CANMessage*)evt.value.p;
hi1000 9:486f65124378 317 memcpy((void *)&tx_message, (void *)message, sizeof(tx_message));
hi1000 8:6105ffbaf237 318
hi1000 9:486f65124378 319 printf("analyzePayload got message id=%d 0x%08x\r\n", tx_message.id, tx_message.id);
hi1000 16:f4277e9b8612 320
hi1000 9:486f65124378 321 for (loop = 0; loop < tx_message.len; loop++)
hi1000 9:486f65124378 322 {
hi1000 9:486f65124378 323 can_rx_data[loop] = tx_message.data[loop];
hi1000 9:486f65124378 324 }
hi1000 9:486f65124378 325
hi1000 9:486f65124378 326 printf("analyzePayload got data: length:%d\r\n", tx_message.len);
hi1000 9:486f65124378 327 for (loop = 0; loop < tx_message.len; loop++)
hi1000 9:486f65124378 328 {
hi1000 9:486f65124378 329 printf("data[%d]=%d\r\n", loop, can_rx_data[loop]);
hi1000 9:486f65124378 330 }
hi1000 16:f4277e9b8612 331 can_id_s.dest_id = (tx_message.id & 0x1FFC0000) >> 18;
hi1000 16:f4277e9b8612 332 can_id_s.source_id = (tx_message.id & 0x0003FF80) >> 7;
hi1000 16:f4277e9b8612 333 can_id_s.message_id = (tx_message.id & 0x0000007F) >> 0;
hi1000 19:0356e54240cc 334 memcpy((void*)&can_rxdata_frame, (void*)can_rx_data, sizeof(can_rx_data));
hi1000 16:f4277e9b8612 335 can_mpool.free(message);
hi1000 16:f4277e9b8612 336 printf("dest_id=%d source_id=%d message_id=%d \r\n", can_id_s.dest_id, can_id_s.source_id, can_id_s.message_id);
hi1000 19:0356e54240cc 337 // can_rxdata_frame.cmd = (can_rx_data[1] << 8 ) | can_rx_data[0];
hi1000 19:0356e54240cc 338 // can_rxdata_frame.value1 = (can_rx_data[3] << 8 ) | can_rx_data[2];
hi1000 19:0356e54240cc 339 // can_rxdata_frame.value2 = (can_rx_data[5] << 8 ) | can_rx_data[4];
hi1000 19:0356e54240cc 340 // can_rxdata_frame.value3 = (can_rx_data[7] << 8 ) | can_rx_data[6];
hi1000 19:0356e54240cc 341 printf("cmd=%d, value1=0x%04x, value2=0x%04x, value3=0x%04x\r\n", can_rxdata_frame.cmd, can_rxdata_frame.value1, can_rxdata_frame.value2, can_rxdata_frame.value3);
hi1000 18:e6ed582f7022 342 device_type_v = (Device_Type_d)device_type;
hi1000 19:0356e54240cc 343 if (can_rxdata_frame.cmd == COMMAND_REGISTER)
hi1000 19:0356e54240cc 344 {
hi1000 19:0356e54240cc 345 can_register_success = true;
hi1000 19:0356e54240cc 346 printf("device register SUCCESS\r\n");
hi1000 19:0356e54240cc 347 }
hi1000 19:0356e54240cc 348 else
hi1000 17:faa4d4976d22 349 {
hi1000 19:0356e54240cc 350 switch (device_type_v)
hi1000 19:0356e54240cc 351 {
hi1000 19:0356e54240cc 352 case CupTrack:
hi1000 19:0356e54240cc 353 printf("CupTrack command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 354 handleCupTrackCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 355 break;
hi1000 19:0356e54240cc 356 case JamTrack:
hi1000 19:0356e54240cc 357 printf("JamTrack command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 358 handleJamTrackCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 359 break;
hi1000 19:0356e54240cc 360 case TeaTrack:
hi1000 19:0356e54240cc 361 printf("TeaTrack command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 362 handleTeaTrackCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 363 break;
hi1000 19:0356e54240cc 364 case Tea:
hi1000 19:0356e54240cc 365 printf("Tea command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 366 handleTeaCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 367 break;
hi1000 19:0356e54240cc 368 case Jam:
hi1000 19:0356e54240cc 369 printf("Jam command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 370 handleJamCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 371 break;
hi1000 19:0356e54240cc 372 case Shaker:
hi1000 19:0356e54240cc 373 printf("Shaker command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 374 handleShakerCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 375 break;
hi1000 19:0356e54240cc 376 case IceMaker:
hi1000 19:0356e54240cc 377 printf("IceMaker command: %d \r\n", can_rxdata_frame.cmd);
hi1000 19:0356e54240cc 378 handleIceMakerCommand(can_rxdata_frame);
hi1000 19:0356e54240cc 379 break;
hi1000 19:0356e54240cc 380 }
hi1000 17:faa4d4976d22 381 }
hi1000 8:6105ffbaf237 382 }
hi1000 20:ec9d4f6a16ac 383 wait(0.0001);
hi1000 8:6105ffbaf237 384 }
hi1000 8:6105ffbaf237 385
hi1000 7:e0c7e624c5fa 386
hi1000 19:0356e54240cc 387 }
hi1000 19:0356e54240cc 388 #endif