File content as of revision 20:ec9d4f6a16ac:

#include "mbed.h"
#include "yoda2.h"
#if 1
extern unsigned char rx_buffer[8], tx_buffer[8];
extern unsigned char rx_length, tx_length;
extern uint8_t can_tx_data[8];
extern uint8_t can_rx_data[8];
extern int current_weight;
extern bool can_register_success;

typedef struct can_id_d {
    int dest_id;
    int source_id;
    int message_id;
};

can_id_d can_id_s;
data_field_d can_rxdata_frame;
data_field_d can_txdata_frame;
CANMessage tx_message;
extern MemoryPool<CANMessage, 16> can_mpool;
extern Queue<CANMessage, 16> can_queue;
extern void can_sendData(int can_id, uint8_t *tx_data, int length);
extern Device_Type_d device_type_v;
extern int device_address;  // last 8 bits of address
extern int device_type; // first 3 bits of adddress

DigitalOut motor1_en(PB_11);
DigitalOut motor1_dir(PB_2);
DigitalOut motor1_pul(PB_1);

DigitalOut motor2_en(PB_15);
DigitalOut motor2_dir(PB_14);
DigitalOut motor2_pul(PB_13);

DigitalOut motor3_en(PC_6);
DigitalOut motor3_dir(PB_6);
DigitalOut motor3_pul(PB_12);

int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome);
int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome);
int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome);

int delay_us (int us)
{
    int ret;
    int i, j;
    for (i =0; i<us; i++)
    {
        for (j = 0; j <10000; j++)
        {
            ret++;
        }    
    }
    return ret;
}
int moveMotor(int motornumber, int distance_mm, bool direction, bool freemove, bool gobackhome)
{
    int ret = 0;
    switch(motornumber)  // 0: cuptrack0  1:cuptrack1   2:cuptrack2 
    {
        case 1:
            ret = moveMotor1(distance_mm, direction, freemove, gobackhome);
        case 2:
            ret = moveMotor2(distance_mm, direction, freemove, gobackhome);       
        case 3:
            ret = moveMotor3(distance_mm, direction, freemove, gobackhome);  
        default:
            return -1;                    
    }
    return ret;
}
int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome)
{
    int loop;
    float mm_per_pulse = 143;
    if (freemove)
    {
        //disable motor
        motor1_en = 1;
        printf("freemove\r\n");
        return 0;    
    }
    motor1_en = 0;
    if (gobackhome)
    {
        //move cup to home    
        printf("go back home\r\n");
        return 0;
    }
    else
    {
        printf("motor1 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
        if (direction) // true: move forward  false: move backward
        {
            //move forward    
            motor1_dir = 1;
            delay_us(400);
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor1_pul = 0;
                delay_us(400);
//                wait(0.00004);
                motor1_pul = 1;
                delay_us(400);
//                wait(0.00004);
            }
        }
        else
        {
            //move backward    
            motor1_dir = 0;
            delay_us(400);
//            wait(0.00001); //5ms
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor1_pul = 0;
                delay_us(400);                
//                wait(0.00003);
                motor1_pul = 1;
                delay_us(400);                
//                wait(0.00003);
            }            
        }
    }
}
int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome)
{
    int loop;
    float mm_per_pulse = 143;
    if (freemove)
    {
        //disable motor
        motor2_en = 1;
        printf("freemove\r\n");
        return 0;    
    }
    motor2_en = 0;
    if (gobackhome)
    {
        //move cup to home    
        printf("go back home\r\n");
        return 0;
    }
    else
    {
        printf("motor2 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
        if (direction) // true: move forward  false: move backward
        {
            //move forward    
            motor2_dir = 1;
            delay_us(400);
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor2_pul = 0;
                delay_us(400);
//                wait(0.00004);
                motor2_pul = 1;
                delay_us(400);
//                wait(0.00004);
            }
        }
        else
        {
            //move backward    
            motor2_dir = 0;
            delay_us(400);
//            wait(0.00001); //5ms
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor2_pul = 0;
                delay_us(400);                
//                wait(0.00003);
                motor2_pul = 1;
                delay_us(400);                
//                wait(0.00003);
            }            
        }
    }
}
int moveMotor3Until(bool direction, DigitalIn sensorpin)
{
    int distance = 0;
    int mm_per_pulse = 143;
    if (direction)
        motor3_dir = 1;
    else
        motor3_dir = 0;
    while(sensorpin.read() == 0)
    {
//        printf("sensorpin %d\r\n", sensorpin.read());
        distance++;
        motor3_pul = 0;
        delay_us(400);                
        //wait(0.00003);
        motor3_pul = 1;
        delay_us(400);                
        //wait(0.00003);       
    }
    return (distance/mm_per_pulse);
}
int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome)
{
    int loop;
    float mm_per_pulse = 143;
    if (freemove)
    {
        //disable motor
        motor3_en = 1;
        printf("freemove\r\n");
        return 0;    
    }
    motor3_en = 0;
    if (gobackhome)
    {
        //move cup to home    
        printf("go back home\r\n");
        return 0;
    }
    else
    {
        printf("motor3 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
        if (direction) // true: move forward  false: move backward
        {
            //move forward    
            motor3_dir = 1;
            delay_us(400);
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor3_pul = 0;
                delay_us(400);
//                wait(0.00004);
                motor3_pul = 1;
                delay_us(400);
//                wait(0.00004);
            }
        }
        else
        {
            //move backward    
            motor3_dir = 0;
            delay_us(400);
//            wait(0.00001); //5ms
            for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
            {
                motor3_pul = 0;
                delay_us(400);                
//                wait(0.00003);
                motor3_pul = 1;
                delay_us(400);                
//                wait(0.00003);
            }            
        }
    }
}

void handleCupTrackCommand(data_field_d data_pack)
{   
}
void handleJamTrackCommand(data_field_d data_pack)
{   
}
void handleTeaTrackCommand(data_field_d data_pack)
{   
}
void handleJamCommand(data_field_d data_pack)
{   
}
void handleTeaCommand(data_field_d data_pack)
{   
    int can_id;
    int init_weight;
    int needed_weight;
    int loop;
    switch (data_pack.cmd)
    {
            case COMMAND_INIT:
                can_txdata_frame.cmd = COMMAND_INIT;
                can_txdata_frame.value1 = current_weight;
                can_txdata_frame.value2 = 0;
                can_txdata_frame.value3 = 0;
                memcpy(can_tx_data, (unsigned char *)&can_txdata_frame, sizeof(can_tx_data));
//                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));
//                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]);  
                can_id = (can_id_s.source_id << 18) | (can_id_s.dest_id << 7) | can_id_s.message_id | 0x80000000;
                can_sendData(can_id, can_tx_data, 8);
                break;
            case COMMAND_PLUS:
                init_weight = current_weight;
                needed_weight = data_pack.value1;
                printf("init_weight:%dg needed_weight:%dg\r\n", init_weight, needed_weight);
                while ((current_weight - init_weight) <= needed_weight)
                {
                    printf("still need to plus weight %dg \r\n", (current_weight - init_weight));
                }
                break;
            default:
                break;
    }
}
void handleShakerCommand(data_field_d command)
{   
}
void handleIceMakerCommand(data_field_d command)
{   
}
void analyzePayload()
{
    int can_id;
    int loop;
    
    printf("analyzePayload thread\r\n");
    while (true) {
        osEvent evt = can_queue.get();
        if (evt.status == osEventMessage) {
            CANMessage *message = (CANMessage*)evt.value.p;
            memcpy((void *)&tx_message, (void *)message, sizeof(tx_message)); 

            printf("analyzePayload got message id=%d 0x%08x\r\n", tx_message.id, tx_message.id);

            for (loop = 0; loop < tx_message.len; loop++)
            {
                can_rx_data[loop] = tx_message.data[loop];
            }
            
            printf("analyzePayload got data: length:%d\r\n", tx_message.len);
            for (loop = 0; loop < tx_message.len; loop++)
            {
                printf("data[%d]=%d\r\n", loop, can_rx_data[loop]);
            }  
            can_id_s.dest_id = (tx_message.id & 0x1FFC0000) >> 18;
            can_id_s.source_id = (tx_message.id & 0x0003FF80) >> 7;
            can_id_s.message_id = (tx_message.id & 0x0000007F) >> 0;
            memcpy((void*)&can_rxdata_frame, (void*)can_rx_data, sizeof(can_rx_data));            
            can_mpool.free(message);   
            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);
//            can_rxdata_frame.cmd = (can_rx_data[1] << 8 ) | can_rx_data[0];
//            can_rxdata_frame.value1 = (can_rx_data[3] << 8 ) | can_rx_data[2];
//            can_rxdata_frame.value2 = (can_rx_data[5] << 8 ) | can_rx_data[4];
//            can_rxdata_frame.value3 = (can_rx_data[7] << 8 ) | can_rx_data[6];
            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);                             
            device_type_v = (Device_Type_d)device_type;
            if (can_rxdata_frame.cmd == COMMAND_REGISTER)
            {
                can_register_success = true;
                printf("device register SUCCESS\r\n");
            }
            else
            {
                switch (device_type_v)
                {
                    case CupTrack:
                        printf("CupTrack command: %d \r\n", can_rxdata_frame.cmd);
                        handleCupTrackCommand(can_rxdata_frame);
                        break;
                    case JamTrack:
                        printf("JamTrack command: %d \r\n", can_rxdata_frame.cmd);
                        handleJamTrackCommand(can_rxdata_frame);
                        break;
                    case TeaTrack:
                        printf("TeaTrack command: %d \r\n", can_rxdata_frame.cmd);                
                        handleTeaTrackCommand(can_rxdata_frame);
                        break;
                    case Tea:
                        printf("Tea command: %d \r\n", can_rxdata_frame.cmd); 
                        handleTeaCommand(can_rxdata_frame);
                        break;
                    case Jam:
                        printf("Jam command: %d \r\n", can_rxdata_frame.cmd); 
                        handleJamCommand(can_rxdata_frame);
                        break;
                    case Shaker:
                        printf("Shaker command: %d \r\n", can_rxdata_frame.cmd);
                        handleShakerCommand(can_rxdata_frame);
                        break;
                    case IceMaker:
                        printf("IceMaker command: %d \r\n", can_rxdata_frame.cmd);
                        handleIceMakerCommand(can_rxdata_frame);
                        break;                        
                }
            }
        }
        wait(0.0001);
    }   
        
    
}
#endif