root / quad1 / AeroQuad / Motors.h @ 9240aaa3
History | View | Annotate | Download (18.7 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
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
|