uncrustify libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde

This commit is contained in:
uncrustify 2012-08-16 23:20:26 -07:00 committed by Pat Hickey
parent 087b2f99c7
commit 2712e5357d
1 changed files with 42 additions and 42 deletions

View File

@ -1,7 +1,7 @@
/*
Example of AP_Motors library.
Code by Randy Mackay. DIYDrones.com
*/
* Example of AP_Motors library.
* Code by Randy Mackay. DIYDrones.com
*/
// AVR runtime
#include <avr/io.h>
@ -46,7 +46,7 @@ RC_Channel rc1, rc2, rc3, rc4;
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsHexa motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsY6 motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
//AP_MotorsOcta motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
@ -56,59 +56,59 @@ AP_MotorsQuad motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
// setup
void setup()
{
Serial.begin(115200);
Serial.println("AP_Motors library test ver 1.0");
{
Serial.begin(115200);
Serial.println("AP_Motors library test ver 1.0");
RC_Channel::set_apm_rc(&APM_RC);
APM_RC.Init( &isr_registry ); // APM Radio initialization
RC_Channel::set_apm_rc(&APM_RC);
APM_RC.Init( &isr_registry ); // APM Radio initialization
// motor initialisation
motors.set_update_rate(490);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_min_throttle(130);
motors.set_max_throttle(850);
motors.Init(); // initialise motors
// motor initialisation
motors.set_update_rate(490);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_min_throttle(130);
motors.set_max_throttle(850);
motors.Init(); // initialise motors
motors.enable();
motors.output_min();
motors.enable();
motors.output_min();
delay(1000);
delay(1000);
}
// loop
void loop()
{
int value;
int value;
// display help
Serial.println("Press 't' to test motors. Becareful they will spin!");
// display help
Serial.println("Press 't' to test motors. Becareful they will spin!");
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
}
// get character from user
value = Serial.read();
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
}
// test motors
if( value == 't' || value == 'T' ) {
Serial.println("testing motors...");
motors.armed(true);
motors.output_test();
motors.armed(false);
Serial.println("finished test.");
}
// get character from user
value = Serial.read();
// test motors
if( value == 't' || value == 'T' ) {
Serial.println("testing motors...");
motors.armed(true);
motors.output_test();
motors.armed(false);
Serial.println("finished test.");
}
}
// print motor output
void print_motor_output()
{
int8_t i;
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if( motors.motor_enabled[i] ) {
Serial.printf("\t%d %d",i,motors.motor_out[i]);
}
}
int8_t i;
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if( motors.motor_enabled[i] ) {
Serial.printf("\t%d %d",i,motors.motor_out[i]);
}
}
}