Raharja Agie
i2c.h@0:d463d5c04541, 2011-08-16 (annotated)
- Committer:
- agiembed
- Date:
- Tue Aug 16 05:32:33 2011 +0000
- Revision:
- 0:d463d5c04541
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| agiembed | 0:d463d5c04541 | 1 | |
| agiembed | 0:d463d5c04541 | 2 | |
| agiembed | 0:d463d5c04541 | 3 | #define ON 8 |
| agiembed | 0:d463d5c04541 | 4 | #define OFF 0 |
| agiembed | 0:d463d5c04541 | 5 | #define ESCMax 254 |
| agiembed | 0:d463d5c04541 | 6 | #define ESCMin 0 |
| agiembed | 0:d463d5c04541 | 7 | |
| agiembed | 0:d463d5c04541 | 8 | const int addr = 0x50; // define the I2C Address |
| agiembed | 0:d463d5c04541 | 9 | char bl_FR[2], bl_BR[2], bl_BL[2], bl_FL[2], all, engine, key = 0, key_st = 0; |
| agiembed | 0:d463d5c04541 | 10 | short front_r = 0, back_r = 0, front_l = 0, back_l = 0; |
| agiembed | 0:d463d5c04541 | 11 | |
| agiembed | 0:d463d5c04541 | 12 | void securinit(){ |
| agiembed | 0:d463d5c04541 | 13 | |
| agiembed | 0:d463d5c04541 | 14 | i2c.write(addr+2, 0, 2); // Send command string to Motor Left |
| agiembed | 0:d463d5c04541 | 15 | i2c.write(addr+4, 0, 2); // Send command string to Motor Back |
| agiembed | 0:d463d5c04541 | 16 | i2c.write(addr+6, 0, 2); // Send command string to Motor Front |
| agiembed | 0:d463d5c04541 | 17 | i2c.write(addr+8, 0, 2); // Send command string to Motor Right |
| agiembed | 0:d463d5c04541 | 18 | return; |
| agiembed | 0:d463d5c04541 | 19 | } |
| agiembed | 0:d463d5c04541 | 20 | |
| agiembed | 0:d463d5c04541 | 21 | void updatei2c(){ |
| agiembed | 0:d463d5c04541 | 22 | /* |
| agiembed | 0:d463d5c04541 | 23 | bl_F[0]= all; |
| agiembed | 0:d463d5c04541 | 24 | bl_B[0]= all; |
| agiembed | 0:d463d5c04541 | 25 | bl_R[0]= all; |
| agiembed | 0:d463d5c04541 | 26 | bl_L[0]= all; |
| agiembed | 0:d463d5c04541 | 27 | */ |
| agiembed | 0:d463d5c04541 | 28 | |
| agiembed | 0:d463d5c04541 | 29 | bl_FR[1]= front_r; |
| agiembed | 0:d463d5c04541 | 30 | bl_BR[1]= back_r; |
| agiembed | 0:d463d5c04541 | 31 | bl_FL[1]= front_l; |
| agiembed | 0:d463d5c04541 | 32 | bl_BL[1]= back_l; |
| agiembed | 0:d463d5c04541 | 33 | |
| agiembed | 0:d463d5c04541 | 34 | i2c.write(addr+2, bl_FR, 2); // Send command string to Motor Left |
| agiembed | 0:d463d5c04541 | 35 | i2c.write(addr+4, bl_BR, 2); // Send command string to Motor Back |
| agiembed | 0:d463d5c04541 | 36 | i2c.write(addr+6, bl_BL, 2); // Send command string to Motor Front |
| agiembed | 0:d463d5c04541 | 37 | i2c.write(addr+8, bl_FL, 2); // Send command string to Motor Right |
| agiembed | 0:d463d5c04541 | 38 | return; |
| agiembed | 0:d463d5c04541 | 39 | } |
| agiembed | 0:d463d5c04541 | 40 | |
| agiembed | 0:d463d5c04541 | 41 | void moteq(){ // motor equation |
| agiembed | 0:d463d5c04541 | 42 | all = 0; |
| agiembed | 0:d463d5c04541 | 43 | |
| agiembed | 0:d463d5c04541 | 44 | if(RC.throttles < 5 && RC.yaws < -200){ |
| agiembed | 0:d463d5c04541 | 45 | if(key_st == 0)key = !key; |
| agiembed | 0:d463d5c04541 | 46 | key_st = 1; |
| agiembed | 0:d463d5c04541 | 47 | } |
| agiembed | 0:d463d5c04541 | 48 | else key_st = 0; |
| agiembed | 0:d463d5c04541 | 49 | if(key == 1) engine = ON; |
| agiembed | 0:d463d5c04541 | 50 | else engine = OFF; |
| agiembed | 0:d463d5c04541 | 51 | |
| agiembed | 0:d463d5c04541 | 52 | |
| agiembed | 0:d463d5c04541 | 53 | |
| agiembed | 0:d463d5c04541 | 54 | front_r = engine + alti_con + yaw_con - pitch_con - roll_con; |
| agiembed | 0:d463d5c04541 | 55 | |
| agiembed | 0:d463d5c04541 | 56 | if(front_r>ESCMax) front_r = ESCMax; |
| agiembed | 0:d463d5c04541 | 57 | else if(front_r<ESCMin+engine) front_r = engine; |
| agiembed | 0:d463d5c04541 | 58 | |
| agiembed | 0:d463d5c04541 | 59 | back_r = engine + alti_con - yaw_con + pitch_con - roll_con; |
| agiembed | 0:d463d5c04541 | 60 | |
| agiembed | 0:d463d5c04541 | 61 | if(back_r>ESCMax) back_r = ESCMax; |
| agiembed | 0:d463d5c04541 | 62 | else if(back_r<ESCMin+engine) back_r = engine; |
| agiembed | 0:d463d5c04541 | 63 | |
| agiembed | 0:d463d5c04541 | 64 | back_l= engine + alti_con + yaw_con + pitch_con + roll_con; |
| agiembed | 0:d463d5c04541 | 65 | |
| agiembed | 0:d463d5c04541 | 66 | if(back_l>ESCMax) back_l = ESCMax; |
| agiembed | 0:d463d5c04541 | 67 | else if(back_l<ESCMin+engine) back_l = engine; |
| agiembed | 0:d463d5c04541 | 68 | |
| agiembed | 0:d463d5c04541 | 69 | front_l = engine + alti_con - yaw_con - pitch_con + roll_con; |
| agiembed | 0:d463d5c04541 | 70 | |
| agiembed | 0:d463d5c04541 | 71 | if(front_l>ESCMax) front_l = ESCMax; |
| agiembed | 0:d463d5c04541 | 72 | else if(front_l<ESCMin+engine) front_l = engine; |
| agiembed | 0:d463d5c04541 | 73 | |
| agiembed | 0:d463d5c04541 | 74 | if(engine == OFF) front_r = back_r = front_l = back_l = 0; |
| agiembed | 0:d463d5c04541 | 75 | |
| agiembed | 0:d463d5c04541 | 76 | } |