int main()
{
char sensor_name[]="mpu";
float rollerr,pitcherr,yawerr;
float prollerr[5],ppitcherr[5],pyawerr[5];
int pcounter=0;
int breakflag=0;
float rsum=0.0;
float psum=0.0;
float ysum=0.0;
//-------- IMU setting -------------------
imu = create_inertial_sensor(sensor_name);
if (!imu) {
printf("Wrong sensor name. Select: mpu or lsm\n");
return EXIT_FAILURE;
}
if (!imu->probe()) {
printf("Sensor not enable\n");
return EXIT_FAILURE;
}
imuSetup();
PWM pwm;
RGBled led;
js_event js;
if(!led.initialize()) return EXIT_FAILURE;
//-------- PS3 Controller setting --------
int joy_fd(-1), num_of_axis(0), num_of_buttons(0);
char name_of_joystick[80];
vector<char> joy_button;
vector<int> joy_axis;
if((joy_fd = open(JOY_DEV, O_RDONLY)) < 0) {
printf("Failed to open %s", JOY_DEV);
cerr << "Failed to open " << JOY_DEV << endl;
return -1;
}
ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
ioctl(joy_fd, JSIOCGNAME(80), &name_of_joystick);
joy_button.resize(num_of_buttons, 0);
joy_axis.resize(num_of_axis, 0);
printf("Joystick: %s axis: %d buttons: %d\n", name_of_joystick, num_of_axis, num_of_buttons);
fcntl(joy_fd, F_SETFL, O_NONBLOCK); // using non-blocking mode
if (check_apm()) {
return 1;
}
for (int i=0; i<10000; i++){
imuLoop();
usleep(1000);
if(i%1000==0){
printf("#");
fflush(stdout);
}
}
for (int i=0;i<4;i++){
if (!pwm.init(i)) {
fprintf(stderr, "Output Enable not set. Are you root?\n");
return 0;
}
pwm.enable(i);
pwm.set_period(i, 500);
}
printf("\nReady to Fly !\n");
////// Log system setting
char filename[] = "flightlog.txt";
char outstr[255];
std::ofstream fs(filename);
char afilename[] = "atest.txt";
char aoutstr[255];
std::ofstream afs(afilename);
char gfilename[] = "gtest.txt";
char goutstr[255];
std::ofstream gfs(gfilename);
char mfilename[] = "mtest.txt";
char moutstr[255];
std::ofstream mfs(mfilename);
///// Clock setting
struct timeval tval;
unsigned long now_time,past_time,interval;
gettimeofday(&tval,NULL);
now_time=1000000 * tval.tv_sec + tval.tv_usec;
past_time = now_time;
interval = now_time - past_time;
//========================== Main Loop ==============================
while(true) {
led.setColor(Colors::Red);
while(true) {
gettimeofday(&tval,NULL);
past_time = now_time;
now_time=1000000 * tval.tv_sec + tval.tv_usec;
interval = now_time - past_time;
//js_event js;
imuLoop();
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
int stickRx=joy_axis[2];
int stickRy=joy_axis[3];
int stickLx=joy_axis[0];
int stickLy=joy_axis[1];
if(stickRx> 23170)stickRx= 23170;
if(stickRx<-23170)stickRx=-23170;
if(stickRy> 23170)stickRy= 23170;
if(stickRy<-23170)stickRy=-23170;
if(stickLx> 23170)stickLx= 23170;
if(stickLx<-23170)stickLx=-23170;
if(stickLy> 23170)stickLy= 23170;
if(stickLy<-23170)stickLy=-23170;
float uthrotle=(-(float)stickRy/23170.0)*1.0+1.0;
float rpitch =( (float)stickLy/23170.0)*10.0;
float rroll =( (float)stickLx/23170.0)*10.0;
float ryaw =( (float)stickRx/23170.0)*10.0;
/*
Itolab Drone Configuration
F
|
|
L----+----R
|
|
B
*/
//------------- Stabilize Control -----------------
rollerr = rroll - roll;
pitcherr= rpitch - pitch;
yawerr = ryaw + gz;
rsum = rsum + rollerr;
psum = psum + pitcherr;
ysum = 0.0;
if (uthrotle<1.05){
rsum = 0;
psum = 0;
}
if(rsum> 20000.0)rsum = 20000.0;
if(rsum<-20000.0)rsum =-20000.0;
if(psum> 20000.0)psum = 20000.0;
if(psum<-20000.0)psum =-20000.0;
//if(ysum>1.0)ysum=1.0;
//if(ysum<0.0)ysum=0.0;
float uroll = 0.005*rollerr + 0.2 *(rollerr - prollerr[pcounter] ) + 0.000001*rsum;
float upitch= 0.01 *pitcherr + 0.4 *(pitcherr- ppitcherr[pcounter]) + 0.000001*psum;
float uyaw = 0.02 *yawerr + 0.01*(yawerr - pyawerr[pcounter] ) ;
prollerr[pcounter] = rollerr;
ppitcherr[pcounter] = pitcherr;
pyawerr[pcounter] = yawerr;
pcounter++;
if(pcounter>4)pcounter=0;
if (uthrotle<1.05){
uroll = 0;
upitch = 0;
uyaw = 0;
}
float R = uthrotle - uroll + uyaw;
float L = uthrotle + uroll + uyaw;
float F = uthrotle + upitch - uyaw;
float B = uthrotle - upitch - uyaw;
//Limitter
if(R>2.0)R=2.0;
if(R<1.0)R=1.0;
if(L>2.0)L=2.0;
if(L<1.0)L=1.0;
if(F>2.0)F=2.0;
if(F<1.0)F=1.0;
if(B>2.0)B=2.0;
if(B<1.0)B=1.0;
pwm.set_duty_cycle(RIGHT_MOTOR, R);
pwm.set_duty_cycle(LEFT_MOTOR, L);
pwm.set_duty_cycle(FRONT_MOTOR, F);
pwm.set_duty_cycle(REAR_MOTOR, B);
//////Logging
//gettimeofday(&tval,NULL);
//past_time = now_time;
//now_time = 1000000 * tval.tv_sec + tval.tv_usec;
//interval = now_time - past_time;
sprintf(outstr,"%lu %lu %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f"
,now_time,interval,
roll,pitch,yaw,
rroll,rpitch,ryaw,
gx,gy,gz,
ax,ay,az,
mx,my,mz,
R,L,F,B
);
fs << outstr << endl;
/*
sprintf(aoutstr,"%lf %lf %lf",ax,ay,az);
afs << aoutstr << std::endl;
sprintf(goutstr,"%lf %lf %lf",gx,gy,gz);
gfs << goutstr << std::endl;
sprintf(moutstr,"%lf %lf %lf",mx,my,mz);
mfs << moutstr << std::endl;
*/
if (joy_button[3]==1){
break;
}
do{
gettimeofday(&tval,NULL);
interval=1000000 * tval.tv_sec + tval.tv_usec - now_time;
}while(interval<2000);
}
led.setColor(Colors::Blue);
while (joy_button[3]==1){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
}
while (true){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
if (joy_button[3]==1){
break;
}
if (joy_button[0]==1){
breakflag=1;
break;
}
else breakflag = 0;
}
if (breakflag == 1)break;
else breakflag = 0;
while (joy_button[3]==1){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
}
}
fs.close();
//afs.close();
//gfs.close();
//mfs.close();
led.setColor(Colors::Green);
return 0;
}
int main()
{
int breakflag=0;
float M = 1.0;
short m = 0;
short cnt = 0;
PWM pwm;
RGBled led;
js_event js;
if(!led.initialize()) return EXIT_FAILURE;
//-------- PS3 Controller setting --------
int joy_fd(-1), num_of_axis(0), num_of_buttons(0);
char name_of_joystick[80];
vector<char> joy_button;
vector<int> joy_axis;
if((joy_fd = open(JOY_DEV, O_RDONLY)) < 0) {
printf("Failed to open %s", JOY_DEV);
cerr << "Failed to open " << JOY_DEV << endl;
return -1;
}
ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
ioctl(joy_fd, JSIOCGNAME(80), &name_of_joystick);
joy_button.resize(num_of_buttons, 0);
joy_axis.resize(num_of_axis, 0);
printf("Joystick: %s axis: %d buttons: %d\n", name_of_joystick, num_of_axis, num_of_buttons);
fcntl(joy_fd, F_SETFL, O_NONBLOCK); // using non-blocking mode
for ( int i = 0 ; i < 4 ; i++ ) {
if ( !pwm.init(i) ) {
fprintf(stderr, "Output Enable not set. Are you root?\n");
return 0;
}
pwm.enable(i);
pwm.set_period(i, 500);
}
printf("\nStart thrust test !\n");
///// Clock setting
struct timeval tval;
unsigned long now_time,past_time,interval;
gettimeofday(&tval,NULL);
now_time=1000000 * tval.tv_sec + tval.tv_usec;
past_time = now_time;
interval = now_time - past_time;
printf("set motor 'RIGHT'\n");
//========================== Main Loop ==============================
while(true) {
led.setColor(Colors::Red);
while(true) {
gettimeofday(&tval,NULL);
past_time = now_time;
now_time=1000000 * tval.tv_sec + tval.tv_usec;
interval = now_time - past_time;
//js_event js;
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
if (joy_button[12]==1){
if (joy_button[4]==1){
M = M + 0.1;
printf ( "PWM UP : %f\n" ,M );
}
if (joy_button[6]==1){
M = M - 0.1;
printf ( "PWM down : %f\n" ,M );
}
}
if (joy_button[13]==1){
if (joy_button[4]==1){
M = M + 0.001;
printf ( "PWM UP : %f\n" ,M );
}
if (joy_button[6]==1){
M = M - 0.001;
printf ( "PWM down : %f\n" ,M );
}
}
if (joy_button[14]==1){
if (joy_button[4]==1){
M = M + 0.0001;
printf ( "PWM UP : %f\n" ,M );
}
if (joy_button[6]==1){
M = M - 0.0001;
printf ( "PWM down : %f\n" ,M );
}
}
if (joy_button[15]==1){
if (joy_button[4]==1){
M = M + 0.00001;
printf ( "PWM UP : %f\n" ,M );
}
if (joy_button[6]==1){
M = M - 0.0001;
printf ( "PWM down : %f\n" ,M );
}
}
if (joy_button[11]==1){
cnt++;
if (cnt>30){
M = 2.0;
printf( "Set PWM MAX!\n" );
cnt = 0;
}
}
if (joy_button[10]==1){
cnt++;
if (cnt>30){
M = 1.0;
printf( "Set PWM MIN!\n" );
cnt = 0;
}
}
if ( M > 2.0 ) M = 2.0;
if ( M < 1.0 ) M = 1.0;
pwm.set_duty_cycle(m, M);
if (joy_button[3]==1){
break;
}
do{
gettimeofday(&tval,NULL);
interval=1000000 * tval.tv_sec + tval.tv_usec - now_time;
}while(interval<2000);
}
led.setColor(Colors::Blue);
while (joy_button[3]==1){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
}
while (true){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
M = 1.0;
if (joy_button[4]==1){
m = 2;
printf("set motor 'FRONT'\n");
}
if (joy_button[5]==1){
m = 0;
printf("set motor 'RIGHT'\n");
}
if (joy_button[6]==1){
m = 3;
printf("set motor 'REAR'\n");
}
if (joy_button[7]==1){
m = 1;
printf("set motor 'LEFT'\n");
}
if (joy_button[3]==1){
break;
}
if (joy_button[0]==1){
breakflag=1;
break;
}
else breakflag = 0;
}
if (breakflag == 1)break;
else breakflag = 0;
while (joy_button[3]==1){
read(joy_fd, &js, sizeof(js_event));
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_AXIS:
joy_axis[(int)js.number] = js.value;
break;
case JS_EVENT_BUTTON:
joy_button[(int)js.number] = js.value;
//printf("%5d\n %5d\n",(int)js.number,js.value);
break;
}
}
}
led.setColor(Colors::Green);
return 0;
}
int main(int argc, char **argv)
{
ROS_INFO("Start");
int saturation = 2000;
int freq = 100;
Kp = 0.6;
Ki = 2;
Kd = 0.01;
servo_trim = 1500;
ROS_INFO("number of argc %d", argc);
if(argc == 1)
{
//case with default params
}
else if(argc == 2)
{
//case with frequency
if(atoi(argv[1]) > 0 )
freq = atoi(argv[1]);
else
{
ROS_INFO("Frequency must be more than 0");
return 0;
}
}
else if(argc == 3)
{
//case with frequency and saturation
if(atoi(argv[1]) > 0 )
freq = atoi(argv[1]);
else
{
ROS_INFO("Frequency must be more than 0");
return 0;
}
if(atoi(argv[2]) > 2000) saturation = 2000;
else saturation = atoi(argv[2]);
}
else if(argc == 6)
{
//case with frequency and saturation
if(atoi(argv[1]) > 0 )
freq = atoi(argv[1]);
else
{
ROS_INFO("Frequency must be more than 0");
return 0;
}
if(atoi(argv[2]) > 2000) saturation = 2000;
else saturation = atoi(argv[2]);
Kp = atof(argv[3]);
Ki = atof(argv[4]);
Kd = atof(argv[5]);
}
else if(argc == 7)
{
//case with frequency and saturation
if(atoi(argv[1]) > 0 )
freq = atoi(argv[1]);
else
{
ROS_INFO("Frequency must be more than 0");
return 0;
}
if(atoi(argv[2]) > 2000) saturation = 2000;
else saturation = atoi(argv[2]);
Kp = atof(argv[3]);
Ki = atof(argv[4]);
Kd = atof(argv[5]);
servo_trim = atoi(argv[6]);
}
else
{
ROS_INFO("not enough arguments ! Specify prbs value and throttle saturation.");
return 0;
}
ROS_INFO("frequency %d, and saturation : %d, Trim", freq, saturation, servo_trim);
/***********************/
/* Initialize The Node */
/***********************/
ros::init(argc, argv, "remote_reading_handler");
ros::NodeHandle n;
ros::Publisher remote_pub = n.advertise<sensor_msgs::Temperature>("remote_readings", 1000);
ros::Publisher control_pub = n.advertise<sensor_msgs::Temperature>("control_readings", 1000);
//subscribe to imu topic
ros::Subscriber imu_sub = n.subscribe("imu_readings", 1000, read_Imu);
//running rate = freq Hz
ros::Rate loop_rate(freq);
/****************************/
/* Initialize the PID Stuff */
/****************************/
currentRoll = 0;
currentTime = ros::Time::now();
previousTime = ros::Time::now();
Kierr = 0;
err = 0;
derr = 0;
derr_filt = 0;
/*******************************************/
/* Initialize the RC input, and PWM output */
/*******************************************/
RCInput rcin;
rcin.init();
PWM servo;
PWM motor;
if (!motor.init(MOTOR_PWM_OUT)) {
fprintf(stderr, "Motor Output Enable not set. Are you root?\n");
return 0;
}
if (!servo.init(SERVO_PWM_OUT)) {
fprintf(stderr, "Servo Output Enable not set. Are you root?\n");
return 0;
}
motor.enable(MOTOR_PWM_OUT);
servo.enable(SERVO_PWM_OUT);
motor.set_period(MOTOR_PWM_OUT, 50); //frequency 50Hz
servo.set_period(SERVO_PWM_OUT, 50);
int motor_input = 0;
int servo_input = 0;
sensor_msgs::Temperature rem_msg;
sensor_msgs::Temperature ctrl_msg;
float desired_roll = 0;
//speed in m/s
float speed = 0;
float speed_filt = 0;
int dtf = 0;// dtf read from arduino. dtf = dt*4 in msec
float R = 0.0625f; //Rear Wheel Radius
while (ros::ok())
{
//Throttle saturation
if(rcin.read(3) >= saturation)
motor_input = saturation;
else
motor_input = rcin.read(3);
//read desired roll angle with remote ( 1250 to 1750 ) to range of -30 to 30 deg
desired_roll = -((float)rcin.read(2)-1500.0f)*MAX_ROLL_ANGLE/250.0f;
ROS_INFO("rcin usec = %d --- desired roll = %f", rcin.read(2), desired_roll);
//calculate output to servo from pid controller
servo_input = pid_Servo_Output(desired_roll);
//write readings on pwm output
motor.set_duty_cycle(MOTOR_PWM_OUT, ((float)motor_input)/1000.0f);
servo.set_duty_cycle(SERVO_PWM_OUT, ((float)servo_input)/1000.0f);
//save values into msg container a
rem_msg.header.stamp = ros::Time::now();
rem_msg.temperature = motor_input;
rem_msg.variance = servo_input;
dtf = rcin.read(4)-1000;
speed = 4.0f*PI*R*1000.0f/((float)dtf);
if(speed < 0 || dtf < 40) speed = 0;
// low pass filtering of the speed with tau = 0.1
float alpha = (1.0f/freq)/((1.0f/freq)+0.1f);
speed_filt = alpha*speed + (1.0f-alpha)*speed_filt;
//save values into msg container for the control readings
ctrl_msg.header.stamp = ros::Time::now();
ctrl_msg.temperature = speed_filt;//_filt;
ctrl_msg.variance = desired_roll;//here it's supposed to be the desired roll
//debug info
printf("[Thrust:%d] - [Steering:%d] - [dtf:%4d] - [Speed:%2.2f]\n", motor_input, servo_input, dtf, speed_filt);
//remote_pub.publish(apub);
remote_pub.publish(rem_msg);
control_pub.publish(ctrl_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
