Raharja Agie
/
Mini-X
i2c.h
- Committer:
- agiembed
- Date:
- 2011-08-16
- Revision:
- 0:d463d5c04541
File content as of revision 0:d463d5c04541:
#define ON 8 #define OFF 0 #define ESCMax 254 #define ESCMin 0 const int addr = 0x50; // define the I2C Address char bl_FR[2], bl_BR[2], bl_BL[2], bl_FL[2], all, engine, key = 0, key_st = 0; short front_r = 0, back_r = 0, front_l = 0, back_l = 0; void securinit(){ i2c.write(addr+2, 0, 2); // Send command string to Motor Left i2c.write(addr+4, 0, 2); // Send command string to Motor Back i2c.write(addr+6, 0, 2); // Send command string to Motor Front i2c.write(addr+8, 0, 2); // Send command string to Motor Right return; } void updatei2c(){ /* bl_F[0]= all; bl_B[0]= all; bl_R[0]= all; bl_L[0]= all; */ bl_FR[1]= front_r; bl_BR[1]= back_r; bl_FL[1]= front_l; bl_BL[1]= back_l; i2c.write(addr+2, bl_FR, 2); // Send command string to Motor Left i2c.write(addr+4, bl_BR, 2); // Send command string to Motor Back i2c.write(addr+6, bl_BL, 2); // Send command string to Motor Front i2c.write(addr+8, bl_FL, 2); // Send command string to Motor Right return; } void moteq(){ // motor equation all = 0; if(RC.throttles < 5 && RC.yaws < -200){ if(key_st == 0)key = !key; key_st = 1; } else key_st = 0; if(key == 1) engine = ON; else engine = OFF; front_r = engine + alti_con + yaw_con - pitch_con - roll_con; if(front_r>ESCMax) front_r = ESCMax; else if(front_r<ESCMin+engine) front_r = engine; back_r = engine + alti_con - yaw_con + pitch_con - roll_con; if(back_r>ESCMax) back_r = ESCMax; else if(back_r<ESCMin+engine) back_r = engine; back_l= engine + alti_con + yaw_con + pitch_con + roll_con; if(back_l>ESCMax) back_l = ESCMax; else if(back_l<ESCMin+engine) back_l = engine; front_l = engine + alti_con - yaw_con - pitch_con + roll_con; if(front_l>ESCMax) front_l = ESCMax; else if(front_l<ESCMin+engine) front_l = engine; if(engine == OFF) front_r = back_r = front_l = back_l = 0; }