/* Program ID : NXTMMX-Tst01.nxc Create date : 5-29-2010 Author : CH, Chen (Taiwan) Description : Functional testing NXTMMX : Register Read Write --------- ---------------------- --------------------- 0x00-0x07 S/W version/V1.01 0x08-0x0f Vendor ID/mndsnsrs 0x10-0x17 Device ID/NxTMMX 0x41 battery vlotage Command Motor 1 setting: 0x42-45 Encoder target(long) Encoder target(long) 0x46 Speed Speed 0x47 Time to run Time to run 0x48 Command register B Command register B(Not use,set 0) 0x49 Command register A Command register A Motor 2 setting: 0x4A-4D Encoder target(long) Encoder target(long) 0x4E Speed Speed 0x4F Time to run Time to run 0x50 Command register B Command register B(Not use,set 0) 0x51 Command register A Command register A Status return: 0x62-65 M1 Encoder position(long) 0x66-69 M2 Encoder position(long) 0x72 M1 status 0x73 M2 status 0x76 M1 task runnung 0x77 M2 task runnung */ #define MMX_PORT S1 //I2C bus and registers #define MMX_I2C_BUS 0x06 #define MMX_BATTERY 0x41 #define MMX_COMMAND 0x41 #define MMX_M1_TACHO 0x42 #define MMX_M1_SPEED 0x46 #define MMX_M1_TIME 0x47 #define MMX_M1_COMMANDA 0x49 #define MMX_M2_TACHO 0x4A #define MMX_M2_SPEED 0x4E #define MMX_M2_TIME 0x4F #define MMX_M2_COMMANDA 0x51 #define MMX_M1_TACHO_CNT 0x62 #define MMX_M2_TACHO_CNT 0x66 #define MMX_M1_STATUS 0x72 #define MMX_M2_STATUS 0x73 #define MMX_M1_TASK_RUN 0x76 #define MMX_M2_TASK_RUN 0x77 //Command A register (0x49,0x51) #define CMD_A_SPEED 0x01 #define CMD_A_RAMP 0x02 #define CMD_A_RELATIVE 0x04 #define CMD_A_TACHO 0x08 #define CMD_A_BRAKE 0x10 #define CMD_A_HOLD_POS 0x20 #define CMD_A_TIME 0x40 #define CMD_A_GO 0x80 //Status register (0x72,0x73) //bit 0-Speed control, bit 1-Ramping , bit 2-Motor is on, bit 3-Position control //bit 4-Braked , bit 5-overloaded, bit 6-Timed mode , bit 7-Stalled #define IS_IN_SPEED_CTRL 0x01 //0000 0001 #define IS_RAMPING 0x02 //0000 0010 #define IS_POWER_ON 0x04 //0000 0100 #define IS_IN_POS_CTRL 0x08 //0000 1000 #define IS_IN_BRAKE 0x10 //0001 0000 #define IS_OVERLOAD 0x20 //0010 0000 #define IS_IN_TIME_CTRL 0x40 //0100 0000 #define IS_STALLED 0x80 //1000 0000 //Motor moving condition #define STALLED_IN_TACHO_MODE 0x8C //1000 1100 #define STALLED_IN_TIME_MODE 0xC4 //1100 0100 //I2C Command (0x41) #define MMX_RESET_ALL 0x52 //R #define MMX_ISSUE_ALL 0x53 //S #define MMX_M1_FLOAT 0x61 //a #define MMX_M2_FLOAT 0x62 //b #define MMX_M12_FLOAT 0x63 //c #define MMX_M1_BRAKE 0x41 //A #define MMX_M2_BRAKE 0x42 //B #define MMX_M12_BRAKE 0x43 //C #define MMX_RESET_M1_TACHO 0x72 //r #define MMX_RESET_M2_TACHO 0x73 //s #define MMX_RESET_ALL_TACHO 0x52 //R //Motor port index #define M_1 0x01 //0000 0001 #define M_2 0x02 //0000 0010 #define M_12 0x03 //0000 0011 byte saveStatus_M1, saveStatus_M2; /* ******************************************************** * Run NXTMMX command(0x41) * ******************************************************** */ void MMX_RunCmd (byte cmd) { byte I2C_buf[]; ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_COMMAND, cmd); while(LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); while(LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); Wait(50); } /* ******************************************************** * Get value of NXTMMX battery voltage register(0x41) * ******************************************************** */ int MMX_GetBattery () { byte nbytes, xstatus=0; byte I2C_get_buf[]; ArrayInit(I2C_get_buf, 0, 1); byte I2C_buf[]; ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_BATTERY); while(LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 1, I2C_buf); while(LowspeedStatus(MMX_PORT, nbytes) == STAT_COMM_PENDING); xstatus = LowspeedRead(MMX_PORT, nbytes, I2C_get_buf); Wait(50); if (xstatus == NO_ERR) return (37 * (I2C_get_buf[0])); else return 0; } /* ******************************************************** * Convert each bit of status register into meaningful characters * ******************************************************** */ string MMX_StatusToStr (byte mmx_status) { string str_status = "--------"; if ((mmx_status & IS_IN_SPEED_CTRL) == IS_IN_SPEED_CTRL) str_status = StrReplace(str_status,0,"S" ); if ((mmx_status & IS_RAMPING) == IS_RAMPING) str_status = StrReplace(str_status,1,"r" ); if ((mmx_status & IS_POWER_ON) == IS_POWER_ON) str_status = StrReplace(str_status,2,"P" ); if ((mmx_status & IS_IN_POS_CTRL) == IS_IN_POS_CTRL) str_status = StrReplace(str_status,3,"E" ); if ((mmx_status & IS_IN_BRAKE) == IS_IN_BRAKE) str_status = StrReplace(str_status,4,"b" ); if ((mmx_status & IS_OVERLOAD) == IS_OVERLOAD) str_status = StrReplace(str_status,5,"o" ); if ((mmx_status & IS_IN_TIME_CTRL) == IS_IN_TIME_CTRL) str_status = StrReplace(str_status,6,"T" ); if ((mmx_status & IS_STALLED) == IS_STALLED) str_status = StrReplace(str_status,7,"s" ); return (str_status); } /* ******************************************************** * Get value of NXTMMX status register(0x72,0x73) * return M1 status * Also pass M2 status by reference parm(m2_status) * ******************************************************** */ byte MMX_GetStatus (byte &m2_status) { byte nbytes, xstatus=0; byte I2C_get_buf[]; ArrayInit(I2C_get_buf, 0, 2); byte I2C_buf[]; ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_M1_STATUS); while(LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 2, I2C_buf); while(LowspeedStatus(MMX_PORT, nbytes) == STAT_COMM_PENDING); xstatus = LowspeedRead(MMX_PORT, nbytes, I2C_get_buf); Wait(50); if (xstatus == NO_ERR) { m2_status = I2C_get_buf[1]; return (I2C_get_buf[0]); } else { m2_status = 0; return 0; } } /* ******************************************************** * Get value of NXTMMX encoder position register(0x62-65,0x66-69) * Return encoder position of M1(long) * Also pass M2 value by reference parm(m2_val) * ******************************************************** */ long MMX_GetTachoCount (long &m2_val) { byte nbytes, xstatus=0; byte I2C_get_buf[]; ArrayInit(I2C_get_buf, 0, 8); byte I2C_buf[]; ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_M1_TACHO_CNT); while(LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 8, I2C_buf); while(LowspeedStatus(MMX_PORT, nbytes) == STAT_COMM_PENDING); xstatus = LowspeedRead(MMX_PORT, nbytes, I2C_get_buf); Wait(50); if (xstatus == NO_ERR) { m2_val = I2C_get_buf[4] + (I2C_get_buf[5]<<8) + (I2C_get_buf[6]<<16) + (I2C_get_buf[7] << 24); return (I2C_get_buf[0] + (I2C_get_buf[1]<<8) + (I2C_get_buf[2]<<16) + (I2C_get_buf[3] << 24)); } else { m2_val = 0; return 0; } } /* ******************************************************** * Check if motor running in tacho limit control is done, * Verify status register(0x72,0x73) bit 2(power on)' 3(encoder control) * Stalled condition is detected on bit 7(on) * return true if both of bit 2' 3 are all off * or the stalled condition is detected (bit 2' 3' 7 are all on) * ******************************************************** */ bool MMX_IsTachoDone (byte mPort) { byte status_M2 = 0; byte status_M1 = MMX_GetStatus(status_M2); saveStatus_M1 = status_M1; saveStatus_M2 = status_M2; bool done_M1 = ((status_M1 & (IS_POWER_ON | IS_IN_POS_CTRL)) == 0?TRUE: ((status_M1 & STALLED_IN_TACHO_MODE) == STALLED_IN_TACHO_MODE?TRUE:FALSE)); bool done_M2 = ((status_M2 & (IS_POWER_ON | IS_IN_POS_CTRL)) == 0?TRUE: ((status_M2 & STALLED_IN_TACHO_MODE) == STALLED_IN_TACHO_MODE?TRUE:FALSE)); switch (mPort) { case M_1: return done_M1; case M_2: return done_M2; case M_12: return (done_M1 && done_M2); default: return TRUE; } } /* ******************************************************** * Check if motor running in time control is done, * Verify status register(0x72,0x73) bit 2(power on)' 6(time control) * Stalled condition is detected on bit 7(on) * return true if both of bit 2' 6 are all off * or the stalled condition is detected (bit 2' 6' 7 are all on) * ******************************************************** */ bool MMX_IsTimeDone (byte mPort) { byte status_M2 = 0; byte status_M1 = MMX_GetStatus(status_M2); saveStatus_M1 = status_M1; saveStatus_M2 = status_M2; bool done_M1 = ((status_M1 & (IS_POWER_ON | IS_IN_TIME_CTRL)) == 0?TRUE: ((status_M1 & STALLED_IN_TIME_MODE) == STALLED_IN_TIME_MODE?TRUE:FALSE)); bool done_M2 = ((status_M2 & (IS_POWER_ON | IS_IN_TIME_CTRL)) == 0?TRUE: ((status_M2 & STALLED_IN_TIME_MODE) == STALLED_IN_TIME_MODE?TRUE:FALSE)); switch (mPort) { case M_1: return done_M1; case M_2: return done_M2; case M_12: return (done_M1 && done_M2); default: return TRUE; } } /* ******************************************************** * Set command A register(0x49,0x51) * mPort : M1-0x01, M2-0x02, Both-0x03 * cmdA : values of command setting * ******************************************************** */ void MMX_SetCmdA (byte mPort, byte cmdA) { byte I2C_buf[]; if ((mPort & M_1) == M_1) { ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_M1_COMMANDA, cmdA); while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if ((mPort & M_2) == M_2) { ArrayBuild(I2C_buf, MMX_I2C_BUS, MMX_M2_COMMANDA, cmdA); while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } } /* ******************************************************** * Set motor run with Tacho limit * mPort : M1-0x01, M2-0x02, Both-0x03 * power_level : speed setting(0x46,0x4E) * tacho_limit : target position setting(0x42-45,0x4A-D) * Command A register setting(0x49,0x51) * is_until_done : run until reached target or stalled * ******************************************************** */ void MMX_MotorRun_Tacho (byte mPort, byte power_level, long tacho_limit, bool is_until_done) { byte I2C_buf[]; ArrayInit(I2C_buf, 0, 10); I2C_buf[0] = MMX_I2C_BUS; I2C_buf[2] = 0xff & tacho_limit; //LSB I2C_buf[3] = 0xff & ((0x0000ff00 & tacho_limit) >> 8); //Byte 2 I2C_buf[4] = 0xff & ((0x00ff0000 & tacho_limit) >> 16); //Byte 3 I2C_buf[5] = 0xff & ((0xff000000 & tacho_limit) >> 24); //MSB I2C_buf[6] = power_level; if (mPort == M_12) I2C_buf[9] = 0x00 | CMD_A_SPEED | CMD_A_TACHO; else I2C_buf[9] = 0x00 | CMD_A_SPEED | CMD_A_TACHO | CMD_A_GO; if ((mPort & M_1) == M_1) { I2C_buf[1] = MMX_M1_TACHO; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if ((mPort & M_2) == M_2) { I2C_buf[1] = MMX_M2_TACHO; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if (mPort == M_12) MMX_RunCmd(MMX_ISSUE_ALL); if (is_until_done) until(MMX_IsTachoDone(mPort)); } /* ******************************************************** * Set motor run with time limit * mPort : M1-0x01, M2-0x02, Both-0x03 * power_level : speed setting * run_seconds : time to run (in seconds) * is_until_done : run until reached time setting or stalled * ******************************************************** */ void MMX_MotorRun_Seconds (byte mPort, byte power_level, byte run_seconds, bool is_until_done) { byte I2C_buf[]; ArrayInit(I2C_buf, 0, 6); I2C_buf[0] = MMX_I2C_BUS; I2C_buf[2] = power_level; I2C_buf[3] = run_seconds; if (mPort == M_12) I2C_buf[5] = 0x00 | CMD_A_SPEED | CMD_A_TIME; else I2C_buf[5] = 0x00 | CMD_A_SPEED | CMD_A_TIME | CMD_A_GO; if ((mPort & M_1) == M_1) { I2C_buf[1] = MMX_M1_SPEED; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if ((mPort & M_2) == M_2) { I2C_buf[1] = MMX_M2_SPEED; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if (mPort == M_12) MMX_RunCmd(MMX_ISSUE_ALL); if (is_until_done) until(MMX_IsTimeDone(mPort)); } /* ******************************************************** * Set motor run without any limit * mPort : M1-0x01, M2-0x02, Both-0x03 * power_level : speed setting * ******************************************************** */ void MMX_MotorRun_Unlimit (byte mPort, byte power_level) { byte I2C_buf[]; ArrayInit(I2C_buf, 0, 6); I2C_buf[0] = MMX_I2C_BUS; I2C_buf[2] = power_level; if (mPort == M_12) I2C_buf[5] = 0x00 | CMD_A_SPEED; else I2C_buf[5] = 0x00 | CMD_A_SPEED | CMD_A_GO; if ((mPort & M_1) == M_1) { I2C_buf[1] = MMX_M1_SPEED; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if ((mPort & M_2) == M_2) { I2C_buf[1] = MMX_M2_SPEED; while (LowspeedCheckStatus(MMX_PORT) == STAT_COMM_PENDING); LowspeedWrite(MMX_PORT, 0, I2C_buf); Wait(50); } if (mPort == M_12) MMX_RunCmd(MMX_ISSUE_ALL); } /* ******************************************************* * Initialization * ******************************************************* */ void Init_rtn() { SetSensorLowspeed(MMX_PORT); } /* ******************************************************** * Run with tacho limt testing scenario * ******************************************************** */ sub Run_TachoLimit_Test () { byte tst_port [], tst_speed [], tst_seconds []; long tst_tacho_limit []; byte status_M1, status_M2; long tacho_cnt_M1, tacho_cnt_M2; ArrayBuild(tst_port, M_1, M_2, M_12, M_1, M_2, M_12); ArrayBuild(tst_speed, 100, 100, 100, 100, 100, 100); ArrayBuild(tst_tacho_limit, -125, -125, 125, -125, -125, 125); ArrayBuild(tst_seconds, 0, 0, 0, 0, 0, 0); byte run_cnt = ArrayLen(tst_port); for (byte xi=0; xi