/* Program ID : NxtServo-tst Create date : 07-20-2008 Author : CH, Chen (Taiwan) Description : 1. Prepare Nxtservo sensor functions call for NXC - Get_Battery_Voltage () - Get_Servo_Position (servo_no) - Set_Servo_Position (servo_no, position) - Set_Servo_Quick_Position (servo_no, quick_position) - Get_Servo_Speed (servo_no) - Set_Servo_Speed (servo_no, speed) - Set_Group_Servo (start_servo_no, count, quick_position) - Reset_Servo () - Store_Servo (servo_no) 2. Pre-defined Macro : - Nxtservo command SERVO_STORE(I) : Store Nth servo current properities(position and speed) to permanent memory SERVO_RESET(S) : Reset servo properities to factory default (position:1500/speed:0) - Register address SERVO_PORT Connect NxtServo sensor to this port DEVICE_ID 0xb0 - I2C address of Nxtservo device SERVO_BATTERY 0x41 - Get external power voltage from this address SERVO_CMD 0x41 - Send device command to this address SERVO_POS 0x42 - Get/Set Servo position to this address SERVO_SPEED 0x52 - Get/Set Servo speed to this address SERVO_Q_POS 0x5a - Set Servo quick change position to this address 3. Servo motor cable plugging: - + Singal SV8 [.][.][.] SV7 [.][.][.] NxtServo Command : -- ---- ---------------------------------------------------- In 0x49 Store initial speed & position of n TH Serveo S 0x53 Reset all I2C Register of NxtServo : Register Read Write --------- --------------------------- --------------------- 0x00-0x07 S/W version 0x08-0x0f mndsnsrs 0x10-0x17 0x41 Battery voltage Command Servo Position Registers : Getting & Setting --------- --------------------------- --------------------- 0x42 Servo 1 position: low byte 0x43 Servo 1 position: high byte 0x44 Servo 2 position: low byte 0x45 Servo 2 position: high byte 0x46 Servo 3 position: low byte 0x47 Servo 3 position: high byte 0x48 Servo 4 position: low byte 0x49 Servo 4 position: high byte 0x4A Servo 5 position: low byte 0x4B Servo 5 position: high byte 0x4C Servo 6 position: low byte 0x4D Servo 6 position: high byte 0x4E Servo 7 position: low byte 0x4F Servo 7 position: high byte 0x50 Servo 8 position: low byte 0x51 Servo 8 position: high byte Speed Registers : Getting & Setting --------- --------------------------- --------------------- 0x52 Speed : Servo 1 (R/W) 0x53 Speed : Servo 2 0x54 Speed : Servo 3 0x55 Speed : Servo 4 0x56 Speed : Servo 5 0x57 Speed : Servo 6 0x58 Speed : Servo 7 0x59 Speed : Servo 8 Quick Registers : Setting only --------- --------------------------- --------------------- 0x5A - Position Servo 1 0x5B - Position Servo 2 0x5C - Position Servo 3 0x5D - Position Servo 4 0x5E - Position Servo 5 0x5F - Position Servo 6 0x60 - Position Servo 7 0x61 - Position Servo 8 */ #define SERVO_PORT S1 //Connect NxtServo sensor to this port #define DEVICE_ID 0xb0 #define SERVO_BATTERY 0x41 #define SERVO_CMD 0x41 #define SERVO_POS 0x42 //0x42~0x51 #define SERVO_SPEED 0x52 //0x52~0x59 #define SERVO_Q_POS 0x5a //0x5A~0x61 #define SERVO_STORE 0x49 #define SERVO_RESET 0x53 #define DELAY 750 //Depends on speed of setting & Servo spec /* ******************************************************** * Get Battery voltage (mV) from register 0x41 * The register contain value 0~255 * where value of 127 corresponds to 4700 mV * ******************************************************** */ int Get_Battery_Voltage () { byte Nxtservo_req_buf[]; ArrayBuild(Nxtservo_req_buf, DEVICE_ID, SERVO_BATTERY); byte Nxtservo_get_buf[]; ArrayInit(Nxtservo_get_buf, 0, 1); char x_status; int x_voltage; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 1, Nxtservo_req_buf); if (x_status < 0) return 0; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedRead(SERVO_PORT, 1, Nxtservo_get_buf); if (x_status < 0) return 0; x_voltage = (Nxtservo_get_buf[0] * 4700) / 127; return x_voltage; } /* ******************************************************** * Get N th Servo positon from registers (0x41,0x42)~(0x50,0x51) * The position is a 16-bit number in uS * ******************************************************** */ int Get_Servo_Position (byte x_servo_no) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 2); byte Nxtservo_get_buf[]; ArrayInit(Nxtservo_get_buf, 0, 2); char x_status; byte x_pos_low, x_pos_high; int x_pos; if ((x_servo_no < 1) || (x_servo_no > 8)) return -1; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_POS + 2*(x_servo_no - 1); while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 2, Nxtservo_req_buf); if (x_status < 0) return 0; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedRead(SERVO_PORT, 2, Nxtservo_get_buf); if (x_status < 0) return 0; x_pos_low = Nxtservo_get_buf[0]; x_pos_high = Nxtservo_get_buf[1]; x_pos = x_pos_high * 255 + x_pos_low; return x_pos; } /* ******************************************************** * Set N th Servo positon to registers (0x41,0x42)~(0x50,0x51) * The position is a 16-bit number in uS * ******************************************************** */ void Set_Servo_Position (byte x_servo_no, int x_pos) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 4); char x_status; byte x_pos_low, x_pos_high; if ((x_servo_no < 1) || (x_servo_no > 8)) return; if (x_pos > 2500) x_pos = 2500; else if (x_pos < 500) x_pos = 500; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_POS + 2*(x_servo_no - 1); x_pos_high = x_pos / 255; x_pos_low = x_pos % 255; Nxtservo_req_buf[2] = x_pos_low; Nxtservo_req_buf[3] = x_pos_high; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); } /* ******************************************************** * Quick set N th Servo positon to registers 0x5A~0x61 * The active value range is 50~250, * where netural position is at 150 * ******************************************************** */ void Set_Servo_Quick_Position (byte x_servo_no, byte x_q_pos) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 3); char x_status; if ((x_servo_no < 1) || (x_servo_no > 8)) return; if (x_q_pos > 250) x_q_pos = 250; else if (x_q_pos < 50) x_q_pos = 50; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_Q_POS + x_servo_no - 1; Nxtservo_req_buf[2] = x_q_pos; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); } /* ******************************************************** * Get N th Servo speed from register 0x52~0x59 * ******************************************************** */ byte Get_Servo_Speed (byte x_servo_no) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 2); byte Nxtservo_get_buf[]; ArrayInit(Nxtservo_get_buf, 0, 1); char x_status; byte x_speed; if ((x_servo_no < 1) || (x_servo_no > 8)) return -1; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_SPEED + x_servo_no - 1; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 1, Nxtservo_req_buf); if (x_status < 0) return 0; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedRead(SERVO_PORT, 1, Nxtservo_get_buf); if (x_status < 0) return 0; x_speed = Nxtservo_get_buf[0]; return x_speed; } /* ******************************************************** * Set N th Servo speed to register 0x52~0x59 * ******************************************************** */ void Set_Servo_Speed (byte x_servo_no, byte x_speed) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 2); char x_status; if ((x_servo_no < 1) || (x_servo_no > 8)) return; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_SPEED + x_servo_no - 1; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); } /* ******************************************************** * Set quick position to servo group * (multiple servos perform same action in the same time) * ******************************************************** */ void Set_Group_Servo (byte x_fm_servo, byte x_servo_cnt, byte x_q_pos) { byte Nxtservo_req_buf[]; byte xii; char x_status; if ((x_fm_servo < 1) || (x_fm_servo > 8)) return; if ((x_fm_servo + x_servo_cnt) > 9) x_servo_cnt = 9 - x_fm_servo; if (x_q_pos > 250) x_q_pos = 250; else if (x_q_pos < 50) x_q_pos = 50; xii = x_servo_cnt + 2; ArrayInit(Nxtservo_req_buf, 0, xii); Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_Q_POS + x_fm_servo - 1; for (xii=2; xii<=(x_fm_servo+x_servo_cnt); xii++) { Nxtservo_req_buf[xii] = x_q_pos; } while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); } /* ******************************************************** * Set all servos position to power-on setting * Position = 1500, speed = 0 * ******************************************************** */ void Set_Servo_Default () { byte xii; int x_position=1500; byte x_speed = 0; for (xii=1; xii<=8; xii++) { Set_Servo_Position(xii, x_position); Set_Servo_Speed(xii, x_speed); Wait(50); } } /* ******************************************************** * Reset servo properities to factory default(position:1500/speed:0) * Using Nxtservo command S(Write 0x53 To address 0x41) * ******************************************************** */ void Reset_Servo() { byte Nxtservo_req_buf[]; ArrayBuild(Nxtservo_req_buf, DEVICE_ID, SERVO_CMD, SERVO_RESET); char x_status; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); Wait(50); } /* ******************************************************** * Store Nth servo current properities(position and speed) * to permanent memory * Using Nxtservo command In(Write 0x49+servo_no To address 0x41) * ******************************************************** */ void Store_Servo(byte x_servo_no) { byte Nxtservo_req_buf[]; ArrayInit(Nxtservo_req_buf, 0, 4); char x_status; if ((x_servo_no < 1) || (x_servo_no > 8)) return; Nxtservo_req_buf[0] = DEVICE_ID; Nxtservo_req_buf[1] = SERVO_CMD; Nxtservo_req_buf[2] = SERVO_STORE; Nxtservo_req_buf[3] = x_servo_no; while(I2CCheckStatus(SERVO_PORT) == STAT_COMM_PENDING); x_status = LowspeedWrite(SERVO_PORT, 0, Nxtservo_req_buf); Wait(50); } /* ******************************************************* * I2C sensor initialization * ******************************************************* */ void I2CSensor_Init() { SetInput(SERVO_PORT, InvalidData, true); SetSensorLowspeed(SERVO_PORT); while(SensorInvalid(SERVO_PORT)); } /* ******************************************************** * Display Servo Register * First 4 register only * ******************************************************** */ void DisplayServo() { byte xii, x_speed; int x_voltage, x_pos; string msg, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8; ClearScreen(); x_voltage = Get_Battery_Voltage(); msg1 = NumToStr(x_voltage); TextOut(0, LCD_LINE1, StrCat("Voltage: ", msg1)); TextOut(0, LCD_LINE2, "Position:"); x_pos = Get_Servo_Position(1); msg1 = NumtoStr(x_pos); x_pos = Get_Servo_Position(2); msg2 = NumtoStr(x_pos); x_pos = Get_Servo_Position(3); msg3 = NumtoStr(x_pos); TextOut(0, LCD_LINE3, StrCat(msg1, " ", msg2, " ", msg3)); x_pos = Get_Servo_Position(4); msg1 = NumtoStr(x_pos); x_pos = Get_Servo_Position(5); msg2 = NumtoStr(x_pos); x_pos = Get_Servo_Position(6); msg3 = NumtoStr(x_pos); TextOut(0, LCD_LINE4, StrCat(msg1, " ", msg2, " ", msg3)); x_pos = Get_Servo_Position(7); msg1 = NumtoStr(x_pos); x_pos = Get_Servo_Position(8); msg2 = NumtoStr(x_pos); TextOut(0, LCD_LINE5, StrCat(msg1, " ", msg2)); TextOut(0, LCD_LINE6, "Speed:"); for (xii=1; xii<=8; xii++) { x_speed = Get_Servo_Speed(xii); if (xii==1) msg1 = NumtoStr(x_speed); else if (xii==2) msg2 = NumtoStr(x_speed); else if (xii==3) msg3 = NumtoStr(x_speed); else if (xii==4) msg4 = NumtoStr(x_speed); else if (xii==5) msg5 = NumtoStr(x_speed); else if (xii==6) msg6 = NumtoStr(x_speed); else if (xii==7) msg7 = NumtoStr(x_speed); else if (xii==8) msg8 = NumtoStr(x_speed); } TextOut(0, LCD_LINE7, StrCat(msg1, " ", msg2, " ", msg3, " ", msg4, " ", msg5, " ", msg6, " ", msg7, " ", msg8)); Wait(5000); //while (!ButtonPressed(BTN4, true)); //wait for ENTER pressed } /* ******************************************************** * Single servo testing routine * ******************************************************** */ void Run_Single_Servo_Test(byte x_seq, byte x_servo, int x_pos, byte x_q_pos, byte x_speed) { int x_voltage=0; string msg; ClearScreen(); msg = NumToStr(x_seq); TextOut(0, LCD_LINE1, StrCat("Single Servo (", msg, ")")); msg = NumToStr(x_servo); TextOut(0, LCD_LINE3, StrCat("Servo: ", msg)); msg = NumToStr(x_pos); TextOut(0, LCD_LINE4, StrCat("Pos: ", msg)); //msg = NumToStr(x_q_pos); //TextOut(0, LCD_LINE4, StrCat("QPos: ", msg)); msg = NumToStr(x_speed); TextOut(0, LCD_LINE5, StrCat("Speed: ", msg)); Set_Servo_Position(x_servo, x_pos); Wait(50); Set_Servo_Speed(x_servo, x_speed); //Set_Servo_Quick_Position(x_servo, x_q_pos); Wait(DELAY); } /* ******************************************************** * Test single servo control functions * ******************************************************** */ void Test_Single_Servo() { byte x_seq_servo[]; ArrayBuild(x_seq_servo, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6); int x_seq_pos[]; ArrayBuild(x_seq_pos, 2500, 500, 1500, 2500, 500, 1500, 2500, 500, 1500, 2500, 500, 1500, 2500, 500, 1500, 2500, 500, 1500); byte x_seq_q_pos[]; ArrayBuild(x_seq_q_pos, 250, 50, 150, 250, 50, 150, 250, 50, 150, 250, 50, 150, 250, 50, 150, 250, 50, 150); byte x_seq_speed[]; ArrayBuild(x_seq_speed, 100, 100, 100, 150, 150, 150, 0, 0, 0, 200, 200, 200, 250, 250, 250, 0, 0, 0); byte x_servo, x_q_pos, x_speed; int x_pos; byte xii; for (xii=0; xii<18; xii++) { x_servo = x_seq_servo[xii]; x_pos = x_seq_pos[xii]; x_q_pos = x_seq_q_pos[xii]; x_speed = x_seq_speed[xii]; Run_Single_Servo_Test(xii, x_servo, x_pos, x_q_pos, x_speed); } //TextOut(0, LCD_LINE7, "Press ENTER.."); //while (!ButtonPressed(BTN4, true)); //wait for ENTER pressed } /* ******************************************************** * Group servo testing routine * ******************************************************** */ void Run_Group_Servo_Test(byte x_seq, byte x_servo, byte x_cnt, byte x_q_pos) { int x_voltage=0; string msg; ClearScreen(); msg = NumToStr(x_seq); TextOut(0, LCD_LINE1, StrCat("Group Servo (", msg, ")")); msg = NumToStr(x_servo); TextOut(0, LCD_LINE3, StrCat("From Servo: ", msg)); msg = NumToStr(x_cnt); TextOut(0, LCD_LINE4, StrCat("Count: ", msg)); msg = NumToStr(x_q_pos); TextOut(0, LCD_LINE5, StrCat("QPos: ", msg)); Set_Group_Servo(x_servo, x_cnt, x_q_pos); Wait(DELAY); } /* ******************************************************** * Test Group servos control functions * ******************************************************** */ void Test_Group_Servo() { byte x_seq_servo[]; ArrayBuild(x_seq_servo, 1, 1, 1, 2, 2, 2, 3, 3, 3, 3, 3, 4, 4, 4, 1, 1, 1); byte x_seq_cnt[]; ArrayBuild(x_seq_cnt, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 6, 6, 6); byte x_seq_q_pos[]; ArrayBuild(x_seq_q_pos, 50, 250, 150, 250, 50, 150, 250, 200, 150, 100, 50, 50, 250, 150, 250, 50, 150); byte x_servo, x_cnt, x_q_pos; byte xii; for (xii=0; xii<17; xii++) { x_servo = x_seq_servo[xii]; x_cnt = x_seq_cnt[xii]; x_q_pos = x_seq_q_pos[xii]; Run_Group_Servo_Test(xii, x_servo, x_cnt, x_q_pos); } //TextOut(0, LCD_LINE7, "Press ENTER.."); //while (!ButtonPressed(BTN4, true)); //wait for ENTER pressed } /* ******************************************************** * main() * ******************************************************** */ task main() { I2CSensor_Init(); DisplayServo(); Set_Servo_Default(); Test_Single_Servo(); Test_Group_Servo(); Set_Servo_Default(); }