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
|