main.cpp

00001 #define BAUD 115200
00002 /*
00003  * rosserial Subscriber Example
00004  * Blinks an LED on callback
00005  */
00006 #include "mbed.h"
00007 #include "rtos.h"
00008 #include <ros.h>
00009 #include <std_msgs/Float32MultiArray.h>
00010 #include <std_msgs/Int32MultiArray.h>
00011 #include "geometry_msgs/Twist.h"
00012 //motor header
00013 #include "MotorDrive.h"
00014 #include "OmniWheel.h"
00015 
00016 using namespace ros;
00017 using namespace std_msgs;
00018 using namespace geometry_msgs;
00019 
00020 
00021 NodeHandle nh;
00022 
00023 DigitalOut myled(LED1);
00024 DigitalOut Refer_Volt_3_3 = PB_1;
00025 DigitalOut motor_enable = PC_7;
00026 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
00027 
00028 
00029 // thread handling wheel
00030 Twist wheel_data; //global
00031 Thread wheel_calc;
00032 Twist save;
00033 
00034 // -> Motor PWN Pin
00035 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
00036 //motor number
00037 const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
00038 //motor pwm period (ms)
00039 const int pwm_period = 20;
00040 //motor stop pwm
00041 const double stop_pwm = 0.5;
00042 
00043 MotorDrive motor_drive;
00044 
00045 void motor_init() {
00046     //init motor
00047     motor_enable = 0;
00048     
00049     for(int i = 0; i < motor_num; i++){
00050         motor_pwm[i].period_ms(pwm_period);
00051         motor_pwm[i] = stop_pwm;
00052     }
00053     //ref voltage
00054     Refer_Volt_3_3 = 1;
00055     wait(1);
00056     motor_enable = 1;
00057 }
00058 
00059 void motorMove(int motor_index, double pwm){
00060     motor_stop[motor_index] = 0;
00061     motor_pwm[motor_index] = pwm;
00062 }
00063 void motorStop(int motor_index){
00064     motor_stop[motor_index] = 1;
00065     motor_pwm[motor_index] = 0.5;
00066 }
00067 float round(float x){
00068     float value = (int)(x*100+0.5);
00069     return (float)value/100;
00070     }
00071 void baseCallback(const Float32MultiArray& base_msg){
00072     double x = (double)round(base_msg.data[1]);
00073     double y = (double)round(base_msg.data[0]);
00074     double z = (double)round(base_msg.data[2]);
00075     OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(x, y, z);
00076     vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
00077     //for(int i = 0; i < motor_num; i++){
00078 //        if(omni_wheel_pwm[i]==0.5)
00079 //            motorStop(i);
00080 //        else
00081 //            motorMove(i, omni_wheel_pwm[i]);
00082 //    }
00083 
00084     if(omni_wheel_pwm[0] == 0.5)
00085         motorStop(0);
00086     else
00087         motorMove(0, omni_wheel_pwm[0]);
00088         
00089     if(omni_wheel_pwm[1] == 0.5)
00090         motorStop(1);
00091     else
00092         motorMove(1, omni_wheel_pwm[1]);
00093         
00094     if(omni_wheel_pwm[2] == 0.5)
00095         motorStop(2);
00096     else
00097         motorMove(2, omni_wheel_pwm[2]);
00098 }
00099 
00100 void messageCb(const Float32MultiArray& toggle_msg){
00101     myled = !myled;   // blink the led
00102 }
00103 
00104 void twCallback(const Twist& _msg) {
00105     OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
00106     
00107     vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();   // Get PWM data
00108     
00109     for(int i = 0; i < motor_num; i++)      // Send PWM to each motor
00110         if(omni_wheel_pwm[i] != stop_pwm)
00111             motorMove(i, omni_wheel_pwm[i]);
00112         else
00113             motorStop(i);
00114 }
00115 
00116 void wheel_calc_thread() {
00117     while (true) {
00118         
00119 bool x = wheel_data.linear.x==save.linear.x;
00120 bool y = wheel_data.linear.y==save.linear.y;
00121 bool z = wheel_data.angular.z==save.angular.z;
00122 
00123 // calc
00124         if(!(x && y && z))
00125         {
00126             twCallback(wheel_data);
00127             wait(5);
00128         }
00129         save = wheel_data;
00130     }
00131     
00132 }
00133 
00134 void saveCallback(const Twist& _msg) {
00135     wheel_data = _msg;
00136 }
00137 
00138 Subscriber<Float32MultiArray> subax("moving_base", &baseCallback);
00139 Subscriber<Float32MultiArray> subbt("fcn_button", &messageCb);
00140 Subscriber<Twist> subtw("base_twist", &saveCallback);
00141 int main() {
00142     //Rate rate(10);
00143     nh.getHardware()->setBaud(BAUD);
00144     nh.initNode();
00145     wheel_calc.start(wheel_calc_thread);
00146 //    nh.subscribe(subax);
00147     nh.subscribe(subbt);
00148     nh.subscribe(subtw);
00149     
00150     motor_init();
00151     while (1) {
00152     nh.spinOnce();
00153  
00154     
00155     if(nh.connected())
00156         myled = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
00157     else 
00158         myled = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
00159     }
00160     
00161 }