i2c.h

00001 
00002 
00003 #define ON 8
00004 #define OFF 0
00005 #define ESCMax 254
00006 #define ESCMin 0
00007 
00008 const int addr = 0x50; // define the I2C Address
00009 char bl_FR[2], bl_BR[2], bl_BL[2], bl_FL[2], all, engine, key = 0, key_st = 0;
00010 short front_r = 0, back_r = 0, front_l = 0, back_l = 0;
00011 
00012 void securinit(){  
00013   
00014     i2c.write(addr+2, 0, 2); // Send command string to Motor Left
00015     i2c.write(addr+4, 0, 2); // Send command string  to Motor Back
00016     i2c.write(addr+6, 0, 2); // Send command string to Motor Front
00017     i2c.write(addr+8, 0, 2); // Send command string   to Motor Right
00018     return;  
00019 }      
00020 
00021 void updatei2c(){
00022     /*
00023     bl_F[0]= all; 
00024     bl_B[0]= all;
00025     bl_R[0]= all;
00026     bl_L[0]= all;
00027     */
00028     
00029     bl_FR[1]= front_r; 
00030     bl_BR[1]= back_r;
00031     bl_FL[1]= front_l;
00032     bl_BL[1]= back_l;
00033     
00034     i2c.write(addr+2, bl_FR, 2); // Send command string to Motor Left
00035     i2c.write(addr+4, bl_BR, 2); // Send command string  to Motor Back
00036     i2c.write(addr+6, bl_BL, 2); // Send command string to Motor Front
00037     i2c.write(addr+8, bl_FL, 2); // Send command string   to Motor Right
00038     return;
00039 }    
00040 
00041 void moteq(){ // motor equation
00042     all = 0;
00043     
00044     if(RC.throttles < 5 && RC.yaws < -200){
00045         if(key_st == 0)key = !key;
00046         key_st = 1;
00047     }
00048     else key_st = 0;
00049     if(key == 1) engine = ON;
00050     else engine = OFF; 
00051 
00052     
00053     
00054     front_r = engine + alti_con + yaw_con - pitch_con - roll_con; 
00055     
00056     if(front_r>ESCMax) front_r = ESCMax;
00057     else if(front_r<ESCMin+engine) front_r = engine;
00058     
00059     back_r = engine + alti_con - yaw_con + pitch_con - roll_con;
00060     
00061     if(back_r>ESCMax) back_r = ESCMax;
00062     else if(back_r<ESCMin+engine) back_r = engine;
00063         
00064     back_l= engine + alti_con + yaw_con + pitch_con + roll_con;
00065     
00066     if(back_l>ESCMax) back_l = ESCMax;
00067     else if(back_l<ESCMin+engine) back_l = engine;
00068         
00069     front_l = engine + alti_con - yaw_con - pitch_con + roll_con;
00070     
00071     if(front_l>ESCMax) front_l = ESCMax;
00072     else if(front_l<ESCMin+engine) front_l = engine;
00073     
00074     if(engine == OFF) front_r = back_r = front_l = back_l = 0;
00075     
00076 }