payload.cpp

00001 #include "mbed.h"
00002 #include "yoda2.h"
00003 #if 1
00004 extern unsigned char rx_buffer[8], tx_buffer[8];
00005 extern unsigned char rx_length, tx_length;
00006 extern uint8_t can_tx_data[8];
00007 extern uint8_t can_rx_data[8];
00008 extern int current_weight;
00009 extern bool can_register_success;
00010 
00011 typedef struct can_id_d {
00012     int dest_id;
00013     int source_id;
00014     int message_id;
00015 };
00016 
00017 can_id_d can_id_s;
00018 data_field_d can_rxdata_frame;
00019 data_field_d can_txdata_frame;
00020 CANMessage tx_message;
00021 extern MemoryPool<CANMessage, 16> can_mpool;
00022 extern Queue<CANMessage, 16> can_queue;
00023 extern void can_sendData(int can_id, uint8_t *tx_data, int length);
00024 extern Device_Type_d device_type_v;
00025 extern int device_address;  // last 8 bits of address
00026 extern int device_type; // first 3 bits of adddress
00027 
00028 DigitalOut motor1_en(PB_11);
00029 DigitalOut motor1_dir(PB_2);
00030 DigitalOut motor1_pul(PB_1);
00031 
00032 DigitalOut motor2_en(PB_15);
00033 DigitalOut motor2_dir(PB_14);
00034 DigitalOut motor2_pul(PB_13);
00035 
00036 DigitalOut motor3_en(PC_6);
00037 DigitalOut motor3_dir(PB_6);
00038 DigitalOut motor3_pul(PB_12);
00039 
00040 int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome);
00041 int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome);
00042 int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome);
00043 
00044 int delay_us (int us)
00045 {
00046     int ret;
00047     int i, j;
00048     for (i =0; i<us; i++)
00049     {
00050         for (j = 0; j <10000; j++)
00051         {
00052             ret++;
00053         }    
00054     }
00055     return ret;
00056 }
00057 int moveMotor(int motornumber, int distance_mm, bool direction, bool freemove, bool gobackhome)
00058 {
00059     int ret = 0;
00060     switch(motornumber)  // 0: cuptrack0  1:cuptrack1   2:cuptrack2 
00061     {
00062         case 1:
00063             ret = moveMotor1(distance_mm, direction, freemove, gobackhome);
00064         case 2:
00065             ret = moveMotor2(distance_mm, direction, freemove, gobackhome);       
00066         case 3:
00067             ret = moveMotor3(distance_mm, direction, freemove, gobackhome);  
00068         default:
00069             return -1;                    
00070     }
00071     return ret;
00072 }
00073 int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome)
00074 {
00075     int loop;
00076     float mm_per_pulse = 143;
00077     if (freemove)
00078     {
00079         //disable motor
00080         motor1_en = 1;
00081         printf("freemove\r\n");
00082         return 0;    
00083     }
00084     motor1_en = 0;
00085     if (gobackhome)
00086     {
00087         //move cup to home    
00088         printf("go back home\r\n");
00089         return 0;
00090     }
00091     else
00092     {
00093         printf("motor1 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
00094         if (direction) // true: move forward  false: move backward
00095         {
00096             //move forward    
00097             motor1_dir = 1;
00098             delay_us(400);
00099             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00100             {
00101                 motor1_pul = 0;
00102                 delay_us(400);
00103 //                wait(0.00004);
00104                 motor1_pul = 1;
00105                 delay_us(400);
00106 //                wait(0.00004);
00107             }
00108         }
00109         else
00110         {
00111             //move backward    
00112             motor1_dir = 0;
00113             delay_us(400);
00114 //            wait(0.00001); //5ms
00115             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00116             {
00117                 motor1_pul = 0;
00118                 delay_us(400);                
00119 //                wait(0.00003);
00120                 motor1_pul = 1;
00121                 delay_us(400);                
00122 //                wait(0.00003);
00123             }            
00124         }
00125     }
00126 }
00127 int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome)
00128 {
00129     int loop;
00130     float mm_per_pulse = 143;
00131     if (freemove)
00132     {
00133         //disable motor
00134         motor2_en = 1;
00135         printf("freemove\r\n");
00136         return 0;    
00137     }
00138     motor2_en = 0;
00139     if (gobackhome)
00140     {
00141         //move cup to home    
00142         printf("go back home\r\n");
00143         return 0;
00144     }
00145     else
00146     {
00147         printf("motor2 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
00148         if (direction) // true: move forward  false: move backward
00149         {
00150             //move forward    
00151             motor2_dir = 1;
00152             delay_us(400);
00153             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00154             {
00155                 motor2_pul = 0;
00156                 delay_us(400);
00157 //                wait(0.00004);
00158                 motor2_pul = 1;
00159                 delay_us(400);
00160 //                wait(0.00004);
00161             }
00162         }
00163         else
00164         {
00165             //move backward    
00166             motor2_dir = 0;
00167             delay_us(400);
00168 //            wait(0.00001); //5ms
00169             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00170             {
00171                 motor2_pul = 0;
00172                 delay_us(400);                
00173 //                wait(0.00003);
00174                 motor2_pul = 1;
00175                 delay_us(400);                
00176 //                wait(0.00003);
00177             }            
00178         }
00179     }
00180 }
00181 int moveMotor3Until(bool direction, DigitalIn sensorpin)
00182 {
00183     int distance = 0;
00184     int mm_per_pulse = 143;
00185     if (direction)
00186         motor3_dir = 1;
00187     else
00188         motor3_dir = 0;
00189     while(sensorpin.read() == 0)
00190     {
00191 //        printf("sensorpin %d\r\n", sensorpin.read());
00192         distance++;
00193         motor3_pul = 0;
00194         delay_us(400);                
00195         //wait(0.00003);
00196         motor3_pul = 1;
00197         delay_us(400);                
00198         //wait(0.00003);       
00199     }
00200     return (distance/mm_per_pulse);
00201 }
00202 int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome)
00203 {
00204     int loop;
00205     float mm_per_pulse = 143;
00206     if (freemove)
00207     {
00208         //disable motor
00209         motor3_en = 1;
00210         printf("freemove\r\n");
00211         return 0;    
00212     }
00213     motor3_en = 0;
00214     if (gobackhome)
00215     {
00216         //move cup to home    
00217         printf("go back home\r\n");
00218         return 0;
00219     }
00220     else
00221     {
00222         printf("motor3 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
00223         if (direction) // true: move forward  false: move backward
00224         {
00225             //move forward    
00226             motor3_dir = 1;
00227             delay_us(400);
00228             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00229             {
00230                 motor3_pul = 0;
00231                 delay_us(400);
00232 //                wait(0.00004);
00233                 motor3_pul = 1;
00234                 delay_us(400);
00235 //                wait(0.00004);
00236             }
00237         }
00238         else
00239         {
00240             //move backward    
00241             motor3_dir = 0;
00242             delay_us(400);
00243 //            wait(0.00001); //5ms
00244             for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
00245             {
00246                 motor3_pul = 0;
00247                 delay_us(400);                
00248 //                wait(0.00003);
00249                 motor3_pul = 1;
00250                 delay_us(400);                
00251 //                wait(0.00003);
00252             }            
00253         }
00254     }
00255 }
00256 
00257 void handleCupTrackCommand(data_field_d data_pack)
00258 {   
00259 }
00260 void handleJamTrackCommand(data_field_d data_pack)
00261 {   
00262 }
00263 void handleTeaTrackCommand(data_field_d data_pack)
00264 {   
00265 }
00266 void handleJamCommand(data_field_d data_pack)
00267 {   
00268 }
00269 void handleTeaCommand(data_field_d data_pack)
00270 {   
00271     int can_id;
00272     int init_weight;
00273     int weight;
00274     int loop;
00275     switch (data_pack.cmd)
00276     {
00277             case COMMAND_INIT:
00278                 can_txdata_frame.cmd = COMMAND_INIT;
00279                 can_txdata_frame.value1 = current_weight;
00280                 can_txdata_frame.value2 = 0;
00281                 can_txdata_frame.value3 = 0;
00282                 memcpy(can_tx_data, (unsigned char *)&can_txdata_frame, sizeof(can_tx_data));
00283 //                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));
00284 //                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]);  
00285                 can_id = (can_id_s.source_id << 18) | (can_id_s.dest_id << 7) | can_id_s.message_id | 0x80000000;
00286                 can_sendData(can_id, can_tx_data, 8);
00287                 break;
00288             case COMMAND_PLUS:
00289                 init_weight = current_weight;
00290                 weight = data_pack.value1;
00291                 printf("init_weight:%dg needed_weight:%dg\r\n", init_weight, weight);
00292                 while ((current_weight - init_weight) <= weight)
00293                 {
00294                     printf("still need to plus weight %dg \r\n", (current_weight - init_weight));
00295                 }
00296                 break;
00297             default:
00298                 break;
00299     }
00300 }
00301 void handleShakerCommand(data_field_d command)
00302 {   
00303 }
00304 void handleIceMakerCommand(data_field_d command)
00305 {   
00306 }
00307 void analyzePayload()
00308 {
00309     int can_id;
00310     int loop;
00311     
00312     printf("analyzePayload thread\r\n");
00313     while (true) {
00314         osEvent evt = can_queue.get();
00315         if (evt.status == osEventMessage) {
00316             CANMessage *message = (CANMessage*)evt.value.p;
00317             memcpy((void *)&tx_message, (void *)message, sizeof(tx_message)); 
00318 
00319             printf("analyzePayload got message id=%d 0x%08x\r\n", tx_message.id, tx_message.id);
00320 
00321             for (loop = 0; loop < tx_message.len; loop++)
00322             {
00323                 can_rx_data[loop] = tx_message.data[loop];
00324             }
00325             
00326             printf("analyzePayload got data: length:%d\r\n", tx_message.len);
00327             for (loop = 0; loop < tx_message.len; loop++)
00328             {
00329                 printf("data[%d]=%d\r\n", loop, can_rx_data[loop]);
00330             }  
00331             can_id_s.dest_id = (tx_message.id & 0x1FFC0000) >> 18;
00332             can_id_s.source_id = (tx_message.id & 0x0003FF80) >> 7;
00333             can_id_s.message_id = (tx_message.id & 0x0000007F) >> 0;
00334             memcpy((void*)&can_rxdata_frame, (void*)can_rx_data, sizeof(can_rx_data));            
00335             can_mpool.free(message);   
00336             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);
00337 //            can_rxdata_frame.cmd = (can_rx_data[1] << 8 ) | can_rx_data[0];
00338 //            can_rxdata_frame.value1 = (can_rx_data[3] << 8 ) | can_rx_data[2];
00339 //            can_rxdata_frame.value2 = (can_rx_data[5] << 8 ) | can_rx_data[4];
00340 //            can_rxdata_frame.value3 = (can_rx_data[7] << 8 ) | can_rx_data[6];
00341             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);                             
00342             device_type_v = (Device_Type_d)device_type;
00343             if (can_rxdata_frame.cmd == COMMAND_REGISTER)
00344             {
00345                 can_register_success = true;
00346                 printf("device register SUCCESS\r\n");
00347             }
00348             else
00349             {
00350                 switch (device_type_v)
00351                 {
00352                     case CupTrack:
00353                         printf("CupTrack command: %d \r\n", can_rxdata_frame.cmd);
00354                         handleCupTrackCommand(can_rxdata_frame);
00355                         break;
00356                     case JamTrack:
00357                         printf("JamTrack command: %d \r\n", can_rxdata_frame.cmd);
00358                         handleJamTrackCommand(can_rxdata_frame);
00359                         break;
00360                     case TeaTrack:
00361                         printf("TeaTrack command: %d \r\n", can_rxdata_frame.cmd);                
00362                         handleTeaTrackCommand(can_rxdata_frame);
00363                         break;
00364                     case Tea:
00365                         printf("Tea command: %d \r\n", can_rxdata_frame.cmd); 
00366                         handleTeaCommand(can_rxdata_frame);
00367                         break;
00368                     case Jam:
00369                         printf("Jam command: %d \r\n", can_rxdata_frame.cmd); 
00370                         handleJamCommand(can_rxdata_frame);
00371                         break;
00372                     case Shaker:
00373                         printf("Shaker command: %d \r\n", can_rxdata_frame.cmd);
00374                         handleShakerCommand(can_rxdata_frame);
00375                         break;
00376                     case IceMaker:
00377                         printf("IceMaker command: %d \r\n", can_rxdata_frame.cmd);
00378                         handleIceMakerCommand(can_rxdata_frame);
00379                         break;                        
00380                 }
00381             }
00382         }
00383         wait(0.0001);
00384     }   
00385         
00386     
00387 }
00388 #endif