Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / Motors.h @ 9240aaa3

History | View | Annotate | Download (18.7 KB)

1
/*
2
  AeroQuad v2.1 - October 2010
3
  www.AeroQuad.com
4
  Copyright (c) 2010 Ted Carancho.  All rights reserved.
5
  An Open Source Arduino based multicopter.
6
 
7
  This program is free software: you can redistribute it and/or modify 
8
  it under the terms of the GNU General Public License as published by 
9
  the Free Software Foundation, either version 3 of the License, or 
10
  (at your option) any later version. 
11

12
  This program is distributed in the hope that it will be useful, 
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of 
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15
  GNU General Public License for more details. 
16

17
  You should have received a copy of the GNU General Public License 
18
  along with this program. If not, see <http://www.gnu.org/licenses/>. 
19
*/
20

    
21
class Motors {
22
public:
23
  // Assume maximum number of motors is 8, leave array indexes unused if lower number
24
  int motorAxisCommand[3];
25
  int motorAxisCommandRoll[8];
26
  int motorAxisCommandPitch[8];
27
  int motorAxisCommandYaw[8];
28
  int motorMixerSettingRoll[8];
29
  int motorMixerSettingPitch[8];
30
  int motorMixerSettingYaw[8];
31
  int motorCommand[8];
32
  int minCommand[8];
33
  int maxCommand[8];
34
  float throttle;
35
  float timerDebug;
36
  int motor;
37
  int delta;
38
  byte axis;
39
  // Ground station control
40
  int remoteCommand[8];
41
  float mMotorCommand;
42
  float bMotorCommand;
43

    
44

    
45
  Motors(void){
46
    throttle = 0;
47
    motorAxisCommand[ROLL] = 0;
48
    motorAxisCommand[PITCH] = 0;
49
    motorAxisCommand[YAW] = 0;
50
    for (motor = 0; motor < 8; motor++) {
51
      motorAxisCommandRoll[motor] = 0;
52
      motorAxisCommandPitch[motor] = 0;
53
      motorAxisCommandYaw[motor] = 0;
54
      motorMixerSettingRoll[motor] = 0;
55
      motorMixerSettingPitch[motor] = 0;
56
      motorMixerSettingYaw[motor] = 0;
57
      motorCommand[motor] = 1000;
58
      minCommand[motor] = MINCOMMAND;
59
      maxCommand[motor] = MAXCOMMAND;
60
      remoteCommand[motor] = 1000;
61
    }
62
    motor = 0;
63
    delta = 0;  
64
  };
65
  
66
  // The following function calls must be defined in any new subclasses
67
  virtual void initialize(void);
68
  virtual void write (void);
69
  virtual void commandAllMotors(int motorCommand);
70
  
71
  //Any number of optional methods can be configured as needed by the SubSystem to expose functionality externally
72
  void pulseMotors(byte quantity) {
73
    for (byte i = 0; i < quantity; i++) {      
74
      commandAllMotors(MINCOMMAND + 100);
75
      delay(250);
76
      commandAllMotors(MINCOMMAND);
77
      delay(250);
78
    }
79
  }
80
  
81
  void setRemoteCommand(byte motor, int value) {
82
    remoteCommand[motor] = value;
83
  }
84
  
85
  const int getRemoteCommand(byte motor) {
86
    return remoteCommand[motor];
87
  }
88
  
89
  const float getMotorSlope(void) {
90
    return mMotorCommand;
91
  }
92
  
93
  const float getMotorOffset(void) {
94
    return bMotorCommand;
95
  }
96
    
97
  void setMinCommand(byte motor, int value) {
98
    minCommand[motor] = value;
99
  }
100
  
101
  const int getMinCommand(byte motor) {
102
    return minCommand[motor];
103
  }
104
  
105
  void setMaxCommand(byte motor, int value) {
106
    maxCommand[motor] = value;
107
  }
108
  
109
  const int getMaxCommand(byte motor) {
110
    return maxCommand[motor];
111
  }
112
  
113
  void setMotorAxisCommand(byte motor, int value) {
114
    motorAxisCommand[motor] = value;
115
  }
116
  
117
  const int getMotorAxisCommand(byte motor) {
118
    return motorAxisCommand[motor];
119
  }
120
  
121
  void setMotorCommand(byte motor, int value) {
122
    motorCommand[motor] = value;
123
  }
124
  
125
  const int getMotorCommand(byte motor) {
126
    return motorCommand[motor];
127
  }
128
  void setThrottle(float value) {
129
    throttle = value;
130
  }
131
  const float getThrottle() {
132
    return throttle;
133
  }
134
};
135

    
136
/******************************************************/
137
/********************* PWM Motors *********************/
138
/******************************************************/
139
class Motors_PWM : public Motors {
140
private:
141
  #if defined(AeroQuadMega_v2) || defined(AeroQuadMega_Wii)
142
    #define FRONTMOTORPIN 2
143
    #define REARMOTORPIN 3
144
    #define RIGHTMOTORPIN 5
145
    #define LEFTMOTORPIN 6
146
    #define LASTMOTORPIN 7
147
  #else
148
    #define FRONTMOTORPIN 3
149
    #define REARMOTORPIN 9
150
    #define RIGHTMOTORPIN 10
151
    #define LEFTMOTORPIN 11
152
    #define LASTMOTORPIN 12
153
  #endif  
154
  int minCommand;
155
  byte pin;
156
  
157
 public:
158
  Motors_PWM() : Motors(){
159
    // Scale motor commands to analogWrite
160
    // Only supports commands from 0-255 => 0 - 100% duty cycle
161
    // Usable pulsewith from approximately 1000-2000 us = 126 - 250        
162
    // m = (250-126)/(2000-1000) = 0.124                
163
    // b = y1 - (m * x1) = 126 - (0.124 * 1000) = 2                
164
    mMotorCommand = 0.124;                
165
    bMotorCommand = 2.0;
166
  }
167

    
168
  void initialize(void) {
169
    pinMode(FRONTMOTORPIN, OUTPUT);
170
    analogWrite(FRONTMOTORPIN, 124);                
171
    pinMode(REARMOTORPIN, OUTPUT);
172
    analogWrite(REARMOTORPIN, 124);                
173
    pinMode(RIGHTMOTORPIN, OUTPUT);
174
    analogWrite(RIGHTMOTORPIN, 124);                
175
    pinMode(LEFTMOTORPIN, OUTPUT);
176
  }
177

    
178
  void write(void) {
179
    analogWrite(FRONTMOTORPIN, (motorCommand[FRONT] * mMotorCommand) + bMotorCommand);
180
    analogWrite(REARMOTORPIN, (motorCommand[REAR] * mMotorCommand) + bMotorCommand);
181
    analogWrite(RIGHTMOTORPIN, (motorCommand[RIGHT] * mMotorCommand) + bMotorCommand);
182
    analogWrite(LEFTMOTORPIN, (motorCommand[LEFT] * mMotorCommand) + bMotorCommand);
183
  }
184
  
185
  void commandAllMotors(int _motorCommand) {   // Sends commands to all motors
186
    analogWrite(FRONTMOTORPIN, (_motorCommand * mMotorCommand) + bMotorCommand);
187
    analogWrite(REARMOTORPIN, (_motorCommand * mMotorCommand) + bMotorCommand);                
188
    analogWrite(RIGHTMOTORPIN, (_motorCommand * mMotorCommand) + bMotorCommand);                
189
    analogWrite(LEFTMOTORPIN, (_motorCommand * mMotorCommand) + bMotorCommand);
190
  }
191
};
192

    
193
/******************************************************/
194
/***************** ArduCopter Motors ******************/
195
/******************************************************/
196
#ifdef ArduCopter
197
class Motors_ArduCopter : public Motors {
198
public:
199
  Motors_ArduCopter() : Motors() {}
200

    
201
  void initialize(void) {
202
    // Init PWM Timer 1
203
    //pinMode(11,OUTPUT); //     (PB5/OC1A)
204
    pinMode(12,OUTPUT); //OUT2 (PB6/OC1B)
205
    pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
206
  
207
    //Remember the registers not declared here remains zero by default... 
208
    TCCR1A =((1<<WGM11)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all... 
209
    TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
210
    //OCR1A = 3000; //PB5, none
211
    OCR1B = 3000; //PB6, OUT2
212
    OCR1C = 3000; //PB7  OUT3
213
    ICR1 = 6600; //300hz freq...
214
  
215
    // Init PWM Timer 3
216
    pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
217
    pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
218
    //pinMode(4,OUTPUT); //     (PE3/OC3A)
219
    TCCR3A =((1<<WGM31)|(1<<COM3B1)|(1<<COM3C1));
220
    TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31); 
221
    //OCR3A = 3000; //PE3, NONE
222
    OCR3B = 3000; //PE4, OUT7
223
    OCR3C = 3000; //PE5, OUT6
224
    ICR3 = 40000; //50hz freq (standard servos)
225
  
226
    // Init PWM Timer 5
227
    pinMode(44,OUTPUT);  //OUT1 (PL5/OC5C)
228
    pinMode(45,OUTPUT);  //OUT0 (PL4/OC5B)
229
    //pinMode(46,OUTPUT);  //     (PL3/OC5A)
230
    
231
    TCCR5A =((1<<WGM51)|(1<<COM5B1)|(1<<COM5C1)); 
232
    TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
233
    //OCR5A = 3000; //PL3, 
234
    OCR5B = 3000; //PL4, OUT0
235
    OCR5C = 3000; //PL5, OUT1
236
    ICR5 = 6600; //300hz freq
237
  
238
    // Init PPM input and PWM Timer 4
239
    pinMode(49, INPUT);  // ICP4 pin (PL0) (PPM input)
240
    pinMode(7,OUTPUT);   //OUT5 (PH4/OC4B)
241
    pinMode(8,OUTPUT);   //OUT4 (PH5/OC4C)
242
        
243
    TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));  
244
    //Prescaler set to 8, that give us a resolution of 0.5us
245
    // Input Capture rising edge
246
    TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
247
    
248
    OCR4A = 40000; ///50hz freq. (standard servos)
249
    OCR4B = 3000; //PH4, OUT5
250
    OCR4C = 3000; //PH5, OUT4
251
   
252
    //TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge). 
253
    //TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
254
    TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
255
  }
256

    
257
  void write (void) {
258
    OCR1B = motorCommand[FRONT] * 2;
259
    OCR1C = motorCommand[REAR] * 2;
260
    OCR5B = motorCommand[RIGHT] * 2;
261
    OCR5C = motorCommand[LEFT] * 2;
262
  }
263

    
264
  void commandAllMotors(int _motorCommand) {   // Sends commands to all motors
265
    OCR1B = _motorCommand * 2;
266
    OCR1C = _motorCommand * 2;
267
    OCR5B = _motorCommand * 2;
268
    OCR5C = _motorCommand * 2;
269
  }
270
};
271
#endif
272

    
273
/*************************************************************/
274
/********************* Multipilot Motors *********************/
275
/*************************************************************/
276
#ifdef MultipilotI2C
277
class Motors_I2C : public Motors {
278
public:
279

    
280

    
281
/* Mixertable VAR */
282
  float MotorGas[LASTMOTOR];
283
  float MotorPitch[LASTMOTOR];
284
  float MotorRoll[LASTMOTOR];
285
  float MotorYaw[LASTMOTOR];
286
  float motorAxisCommandPitch[LASTMOTOR];
287
  float motorAxisCommandRoll[LASTMOTOR];
288
  float motorAxisCommandYaw[LASTMOTOR];
289
  
290
  unsigned char MotorI2C[LASTMOTOR];        
291
  Motors_I2C() : Motors() {}
292

    
293
  void initialize(void) {
294
  char Motor[LASTMOTOR];                
295
  // Scale motor commands to analogWrite                
296
  // m = (250-126)/(2000-1000) = 0.124                
297
  // b = y1 - (m * x1) = 126 - (0.124 * 1000) = 2                
298
  //float mMotorCommand = 0.124;                
299
  //float bMotorCommand = 2 ;
300
  
301
  mMotorCommand = 0.255;                
302
  bMotorCommand = -255;
303
  timer_debug=0;
304
  
305
  //motorCommand[8] = {1000,1000,1000,1000,1000,1000,1000,1000};
306

    
307
  //int subtrim[4] = {1500,1500,1500,1500};    //SUBTRIM li esprimo in millisecondi come i servi per standardizzazione. 
308
  //motorAxisCommand[3] = {0,0,0};
309
  
310
  // If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V 
311
  // Scale gyro output (-465 to +465) to motor commands (1000 to 2000) 
312
  // use y = mx + b 
313
  //mMotorRate = 1.0753; // m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465)) 
314
  //bMotorRate = 1500;   // b = y1 - m * x1
315
  init_mixer_table(); // Init MixerTable
316
  Wire.begin(0x29);
317
 }
318

    
319
// C'e' im
320
#ifdef HEXARADIAL
321
void init_mixer_table()
322
{
323
// Example for Hexa configuration
324
MotorGas[0] = 100;
325
MotorPitch[0] = -100;  
326
MotorRoll[0] =  0;  
327
MotorYaw[0] =  100;  
328

    
329
MotorGas[1] = 100;
330
MotorPitch[1] = -50;  
331
MotorRoll[1] =  -100;  
332
MotorYaw[1] =  -100;  
333

    
334
MotorGas[2] = 100;
335
MotorPitch[2] = +50  ; 
336
MotorRoll[2] =  -100;  
337
MotorYaw[2] =  100;  
338

    
339
MotorGas[3] = 100;
340
MotorPitch[3] =  +100;  
341
MotorRoll[3] =  0;  
342
MotorYaw[3] =  -100;  
343

    
344
MotorGas[4] = 100;
345
MotorPitch[4] = +50;  
346
MotorRoll[4] =  100;  
347
MotorYaw[4] =  100;  
348

    
349
MotorGas[5] = 100;
350
MotorPitch[5] =  -50;  
351
MotorRoll[5] =  100;  
352
MotorYaw[5] =  -100;  
353
}
354
#endif
355

    
356
// Example of Hexa Coaxial.
357
//Configurazione Motori Hexa Coaxial
358

    
359
//Dietro                         GAS                NICK                        ROLL                        YAW
360
//Sopra 1                         64                -64                        0                        -64
361
//Sotto 2                        76                -64                        0                        64
362

    
363
//Guardando l'Hexafox da dietro braccio  Sinistro
364
//Sopra 3                        64                32                        64                        64
365
//Sotto 4                        76                32                        64                        -64
366

    
367
//Guardando l'Hexafox da dietro braccio  Destro.
368

    
369
//Sopra 5                        64                32                        -64                        64
370
//Sotto 6                        76                32                        -64                        -64
371

    
372

    
373
#ifdef HEXACOAXIAL
374
void init_mixer_table()
375
{
376
// Example for Hexa configuration
377
MotorGas[0] = 95;
378
MotorPitch[0] = 100;  
379
MotorRoll[0] =  0;  
380
MotorYaw[0] =  100; 
381

    
382
MotorGas[1] = 100;
383
MotorPitch[1] = 100;  
384
MotorRoll[1] =  0;  
385
MotorYaw[1] =  -100;  
386

    
387
MotorGas[2] = 95;
388
MotorPitch[2] = -50  ; 
389
MotorRoll[2] =  100;  
390
MotorYaw[2] =  -100;  
391

    
392
MotorGas[3] = 100;
393
MotorPitch[3] =  -50;  
394
MotorRoll[3] =  100;  
395
MotorYaw[3] =  100;  
396

    
397
MotorGas[4] = 95;
398
MotorPitch[4] = -50;  
399
MotorRoll[4] =  -100; 
400
MotorYaw[4] =  -100;  
401

    
402
MotorGas[5] = 100;
403
MotorPitch[5] =  -50;  
404
MotorRoll[5] =  -100;  
405
MotorYaw[5] =  100;  
406
}
407
#endif
408

    
409

    
410
void motor_axis_correction()
411
{
412
int i;
413
for (i=0;i<LASTMOTOR;i++)
414
  {
415
  motorAxisCommandPitch[i] = (motorAxisCommand[PITCH] / 100.0) * MotorPitch[i];
416
  motorAxisCommandRoll[i] = (motorAxisCommand[ROLL] / 100.0) * MotorRoll[i];
417
  motorAxisCommandYaw[i] = (motorAxisCommand[YAW] / 100.0) * MotorYaw[i];
418
  }
419
}
420

    
421
//After that we can mix them together:
422
void motor_matrix_command()
423
{
424
int i;
425
float valuemotor;
426
for (i=0;i<LASTMOTOR;i++)
427
  {
428
   valuemotor = ((Throttle* MotorGas[i])/100) + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i];
429
   //valuemotor = Throttle + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i]; // OLD VERSION WITHOUT GAS CONTROL ON Mixertable
430
   valuemotor = constrain(valuemotor, minAcro, MAXCOMMAND);
431
   motorCommand[i]=valuemotor;
432
  }
433
}
434

    
435

    
436
void matrix_debug()
437
{
438
#ifdef PRINT_MIXERTABLE 
439
     Serial.println();
440
     Serial.println("--------------------------");
441
     Serial.println("        Motori Mixertable " );
442
     Serial.println("--------------------------");
443
     Serial.println();
444

    
445
/*
446
     Serial.print("AIL:");
447
     Serial.print(ch1);
448
     Serial.print(" ELE:");
449
     Serial.print(ch2);
450
*/
451
     Serial.print(" THR:");
452
     Serial.print(Throttle);
453
/*
454
     Serial.print(" YAW:");
455
     Serial.print(ch4);
456
     Serial.print(" AUX:");
457
     Serial.print(ch_aux);
458
     Serial.print(" AUX2:");
459
     Serial.print(ch_aux2);
460
  */
461
     Serial.println();
462
     Serial.print("CONTROL_ROLL:");
463
     Serial.print(motorAxisCommand[ROLL]);
464
     Serial.print(" CONTROL_PITCH:");
465
     Serial.print(motorAxisCommand[PITCH]);
466
     Serial.print(" CONTROL_YAW:");
467
     Serial.print(motorAxisCommand[YAW]);
468
//     Serial.print(" SONAR_VALUE:");
469
//     Serial.print(sonar_value);
470
//     Serial.print(" TARGET_SONAR_VALUE:");
471
//     Serial.print(target_sonar_altitude);
472
//     Serial.print(" ERR_SONAR_VALUE:");
473
//     Serial.print(err_altitude);
474
//     Serial.println();
475
//     Serial.print("latitude:");
476
//     Serial.print(GPS_np.Lattitude);
477
//     Serial.print(" longitude:");
478
//     Serial.print(GPS_np.Longitude);
479
//     Serial.print(" command gps roll:");
480
//     Serial.print(command_gps_roll);
481
//     Serial.print(" command gps pitch:");
482
//     Serial.print(command_gps_pitch);
483
//     Serial.print(" Lon_diff:");
484
//     Serial.print(Lon_diff);
485
//     Serial.print(" Lon_diff");
486
//     Serial.print(command_gps_pitch);
487
 
488
//     Serial.println();
489
     
490
//     Serial.print("AP MODE:");Serial.print((int)AP_mode);
491

    
492
#ifdef ARDUCOPTER
493
     Serial.print("AIL:");
494
     Serial.print(ch1);
495
     Serial.print(" ELE:");
496
     Serial.print(ch2);
497
     Serial.print(" THR:");
498
     Serial.print(ch3);
499
     Serial.print(" YAW:");
500
     Serial.print(ch4);
501
     Serial.print(" AUX:");
502
     Serial.print(ch_aux);
503
     Serial.print(" AUX2:");
504
     Serial.print(ch_aux2);
505
     Serial.println();
506
     Serial.print("CONTROL_ROLL:");
507
     Serial.print(control_roll);
508
     Serial.print(" CONTROL_PITCH:");
509
     Serial.print(control_pitch);
510
     Serial.print(" CONTROL_YAW:");
511
     Serial.print(control_yaw);
512
     Serial.print(" SONAR_VALUE:");
513
     Serial.print(sonar_value);
514
     Serial.print(" TARGET_SONAR_VALUE:");
515
     Serial.print(target_sonar_altitude);
516
     Serial.print(" ERR_SONAR_VALUE:");
517
     Serial.print(err_altitude);
518
     Serial.println();
519
     Serial.print("latitude:");
520
     Serial.print(GPS_np.Lattitude);
521
     Serial.print(" longitude:");
522
     Serial.print(GPS_np.Longitude);
523
     Serial.print(" command gps roll:");
524
     Serial.print(command_gps_roll);
525
     Serial.print(" command gps pitch:");
526
     Serial.print(command_gps_pitch);
527
     Serial.print(" Lon_diff:");
528
     Serial.print(Lon_diff);
529
     Serial.print(" Lon_diff");
530
     Serial.print(command_gps_pitch);
531
 
532
     Serial.println();
533
     
534
     Serial.print("AP MODE:");Serial.print((int)AP_mode);
535
#endif
536

    
537
#ifdef HEXARADIAL
538
     Serial.println();
539
     Serial.print((unsigned int)MotorI2C[5]);
540
     comma();
541
     Serial.print((unsigned int)MotorI2C[0]);
542
     comma();
543
     Serial.print((unsigned int)MotorI2C[1]);
544
     comma();
545
     Serial.println();
546
     Serial.print((unsigned int)MotorI2C[4]);
547
     comma();
548
     Serial.print((unsigned int)MotorI2C[3]);
549
     comma();
550
     Serial.println((unsigned int)MotorI2C[2]);
551
     Serial.println("---------------");
552
     Serial.println();
553
#endif
554

    
555
// Example of Hexa Coaxial.
556
//Configurazione Motori Hexa Coaxial
557

    
558
//Dietro                         GAS                NICK                        ROLL                        YAW
559
//Sopra 1                         64                -64                        0                        -64
560
//Sotto 2                        76                -64                        0                        64
561

    
562
//Guardando l'Hexafox da dietro braccio  Sinistro
563
//Sopra 3                        64                32                        64                        64
564
//Sotto 4                        76                32                        64                        -64
565

    
566
//Guardando l'Hexafox da dietro braccio  Destro.
567

    
568
//Sopra 5                        64                32                        -64                        64
569
//Sotto 6                        76                32                        -64                        -64
570

    
571

    
572
#ifdef HEXACOAXIAL
573
     Serial.println();
574
     Serial.print((unsigned int)MotorI2C[2]);
575
     comma();
576
     Serial.print((unsigned int)MotorI2C[4]);
577
     Serial.println();
578
     //comma();
579
     Serial.print((unsigned int)MotorI2C[3]);
580
     comma();
581
     Serial.print((unsigned int)MotorI2C[5]);
582
     Serial.println();
583
     Serial.print ("   ");
584
     //comma();
585
     Serial.print((unsigned int)MotorI2C[0]);
586
     Serial.println();
587
     Serial.print ("   ");
588
     //comma();
589
     Serial.println((unsigned int)MotorI2C[1]);
590
     Serial.println("---------------");
591
     Serial.println();
592
#endif
593

    
594
#endif
595
}
596

    
597
void WireMotorWrite()
598
{
599
int i = 0;
600
int nmotor=0;
601
int index=0;
602
int tout=0;
603
char buff_i2c[10];
604

    
605
Wire.endTransmission(); //end transmission
606
for(nmotor=0;nmotor<6;nmotor++)
607
  {
608
  index=0x29+nmotor;
609
  Wire.beginTransmission(index);
610
  Wire.send(MotorI2C[nmotor]); 
611
  Wire.endTransmission(); //end transmission
612
  Wire.requestFrom(index, 1);    // request 6 bytes from device
613
  i=0;
614
  while(1)   
615
  //while((Wire.available())&&(i<6))
616
  { 
617
    buff_i2c[i] = Wire.receive();  // receive one byte
618
    i++;
619
    if (i>6)break; 
620
    //Serial.print(i);
621
    if (Wire.available()==0)break;
622
  }
623
 
624
  }
625

    
626
}
627
  void write (void) {
628
    // Matrix transformation.
629
    motor_axis_correction();
630
    // Matrix Command.  
631
    motor_matrix_command();
632
   // Matrix Assignment. 
633
    MotorI2C[MOTORID1]=(char)((motorCommand[0] * mMotorCommand) + bMotorCommand); 
634
    MotorI2C[MOTORID2]=(char)((motorCommand[1] * mMotorCommand) + bMotorCommand);
635
    MotorI2C[MOTORID3]=(char)((motorCommand[2] * mMotorCommand) + bMotorCommand);
636
    MotorI2C[MOTORID4]=(char)((motorCommand[3] * mMotorCommand) + bMotorCommand);
637
    MotorI2C[MOTORID5]=(char)((motorCommand[4] * mMotorCommand) + bMotorCommand);
638
    MotorI2C[MOTORID6]=(char)((motorCommand[5] * mMotorCommand) + bMotorCommand); 
639
  
640
   if((millis()-timer_debug)>1000)   // 100ms => 10 Hz loop rate 
641
  {
642
    timer_debug=millis();
643
    #ifdef TELEMETRY_DEBUG
644
    matrix_debug();
645
    #endif     
646
}
647
   
648
 
649
    WireMotorWrite();
650
}
651

    
652
  void commandAllMotors(int _motorCommand) {   // Sends commands to all motors
653
    
654
    // Matrix transformation.
655
    motor_axis_correction();
656
    // Matrix Command.  
657
    motor_matrix_command();
658
   // Matrix Assignment. 
659
    MotorI2C[MOTORID1]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
660
    MotorI2C[MOTORID2]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
661
    MotorI2C[MOTORID3]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
662
    MotorI2C[MOTORID4]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
663
    MotorI2C[MOTORID5]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
664
    MotorI2C[MOTORID6]=(char)((_motorCommand * mMotorCommand) + bMotorCommand);
665
    matrix_debug();
666
    WireMotorWrite();
667
  }
668
};
669
#endif