![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fyp base control
Dependencies: mbed mbed-rtos ros_lib_melodic_
main.cpp@3:5402a080282c, 23 months ago (annotated)
- Committer:
- howanglam3
- Date:
- Thu Aug 18 07:35:33 2022 +0000
- Revision:
- 3:5402a080282c
- Parent:
- 1:782534ae7166
add battery monitor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
howanglam3 | 3:5402a080282c | 1 | #define BAUD 256000 |
howanglam3 | 0:54452c8078db | 2 | #include "mbed.h" |
howanglam3 | 0:54452c8078db | 3 | #include "math.h" |
howanglam3 | 1:782534ae7166 | 4 | #include "rtos.h" |
howanglam3 | 0:54452c8078db | 5 | #include <ros.h> |
howanglam3 | 0:54452c8078db | 6 | #include "std_msgs/Bool.h" |
howanglam3 | 0:54452c8078db | 7 | #include "std_msgs/String.h" |
howanglam3 | 0:54452c8078db | 8 | #include "geometry_msgs/Twist.h" |
howanglam3 | 0:54452c8078db | 9 | |
howanglam3 | 0:54452c8078db | 10 | |
howanglam3 | 0:54452c8078db | 11 | using namespace ros; |
howanglam3 | 0:54452c8078db | 12 | using namespace std_msgs; |
howanglam3 | 0:54452c8078db | 13 | using namespace geometry_msgs; |
howanglam3 | 0:54452c8078db | 14 | |
howanglam3 | 0:54452c8078db | 15 | |
howanglam3 | 0:54452c8078db | 16 | NodeHandle nh; |
howanglam3 | 1:782534ae7166 | 17 | Thread battery_thread; |
howanglam3 | 1:782534ae7166 | 18 | |
howanglam3 | 1:782534ae7166 | 19 | AnalogIn battery(A0); |
howanglam3 | 0:54452c8078db | 20 | |
howanglam3 | 0:54452c8078db | 21 | DigitalOut myled = LED1; |
howanglam3 | 0:54452c8078db | 22 | // -> Motor PWN Pin |
howanglam3 | 1:782534ae7166 | 23 | PwmOut motor_pwm[] = {D6, D5, D3, D4}; |
howanglam3 | 3:5402a080282c | 24 | |
howanglam3 | 0:54452c8078db | 25 | DigitalOut LED01 = D7; |
howanglam3 | 0:54452c8078db | 26 | DigitalOut LED02 = D8; |
howanglam3 | 0:54452c8078db | 27 | DigitalOut LED03 = D9; |
howanglam3 | 0:54452c8078db | 28 | //motor number |
howanglam3 | 3:5402a080282c | 29 | double round(double number) |
howanglam3 | 3:5402a080282c | 30 | { |
howanglam3 | 3:5402a080282c | 31 | return number < 0.0 ? ceil(number - 0.5): floor(number + 0.5); |
howanglam3 | 3:5402a080282c | 32 | }; |
howanglam3 | 0:54452c8078db | 33 | |
howanglam3 | 1:782534ae7166 | 34 | void turn(float r, float l){ |
howanglam3 | 1:782534ae7166 | 35 | float a, b, c, d; |
howanglam3 | 1:782534ae7166 | 36 | if (l < 0){ |
howanglam3 | 1:782534ae7166 | 37 | a = 0.0; |
howanglam3 | 3:5402a080282c | 38 | b = abs(l); |
howanglam3 | 1:782534ae7166 | 39 | } |
howanglam3 | 1:782534ae7166 | 40 | else{ |
howanglam3 | 1:782534ae7166 | 41 | a = l; |
howanglam3 | 1:782534ae7166 | 42 | b = 0.0; |
howanglam3 | 1:782534ae7166 | 43 | } |
howanglam3 | 1:782534ae7166 | 44 | if (r < 0){ |
howanglam3 | 1:782534ae7166 | 45 | c = 0.0; |
howanglam3 | 3:5402a080282c | 46 | d = abs(r); |
howanglam3 | 1:782534ae7166 | 47 | } |
howanglam3 | 1:782534ae7166 | 48 | else{ |
howanglam3 | 1:782534ae7166 | 49 | c = r; |
howanglam3 | 1:782534ae7166 | 50 | d = 0.0; |
howanglam3 | 1:782534ae7166 | 51 | } |
howanglam3 | 1:782534ae7166 | 52 | motor_pwm[0]=a; |
howanglam3 | 1:782534ae7166 | 53 | motor_pwm[1]=b; |
howanglam3 | 1:782534ae7166 | 54 | motor_pwm[2]=c; |
howanglam3 | 1:782534ae7166 | 55 | motor_pwm[3]=d; |
howanglam3 | 0:54452c8078db | 56 | } |
howanglam3 | 0:54452c8078db | 57 | |
howanglam3 | 1:782534ae7166 | 58 | void resize(float &a, float &b){ |
howanglam3 | 1:782534ae7166 | 59 | if (a != 0 or b != 0) { |
howanglam3 | 1:782534ae7166 | 60 | if (b > a){ |
howanglam3 | 1:782534ae7166 | 61 | a = a/b; |
howanglam3 | 1:782534ae7166 | 62 | if (b > 1) |
howanglam3 | 1:782534ae7166 | 63 | b = 1; |
howanglam3 | 1:782534ae7166 | 64 | } |
howanglam3 | 1:782534ae7166 | 65 | else{ |
howanglam3 | 1:782534ae7166 | 66 | b = b/a; |
howanglam3 | 1:782534ae7166 | 67 | if (a > 1) |
howanglam3 | 1:782534ae7166 | 68 | a = 1; |
howanglam3 | 1:782534ae7166 | 69 | } |
howanglam3 | 1:782534ae7166 | 70 | } |
howanglam3 | 1:782534ae7166 | 71 | |
howanglam3 | 0:54452c8078db | 72 | } |
howanglam3 | 1:782534ae7166 | 73 | |
howanglam3 | 0:54452c8078db | 74 | void r1Callback(const Bool& _msg){ |
howanglam3 | 0:54452c8078db | 75 | bool check = _msg.data; |
howanglam3 | 0:54452c8078db | 76 | if(check){ |
howanglam3 | 0:54452c8078db | 77 | LED01 = 1; |
howanglam3 | 0:54452c8078db | 78 | LED02 = 1; |
howanglam3 | 0:54452c8078db | 79 | LED03 = 1;} |
howanglam3 | 0:54452c8078db | 80 | else{ |
howanglam3 | 0:54452c8078db | 81 | LED01 = 0; |
howanglam3 | 0:54452c8078db | 82 | LED02 = 0; |
howanglam3 | 0:54452c8078db | 83 | LED03 = 0;} |
howanglam3 | 0:54452c8078db | 84 | } |
howanglam3 | 0:54452c8078db | 85 | |
howanglam3 | 1:782534ae7166 | 86 | float sum = 0.0; |
howanglam3 | 1:782534ae7166 | 87 | //std_msgs::String motor_msg; |
howanglam3 | 1:782534ae7166 | 88 | //char buffer[50]; |
howanglam3 | 1:782534ae7166 | 89 | //Publisher motor_pub("motor", &motor_msg); |
howanglam3 | 1:782534ae7166 | 90 | |
howanglam3 | 1:782534ae7166 | 91 | |
howanglam3 | 0:54452c8078db | 92 | void twCallback(const Twist& _msg) { |
howanglam3 | 3:5402a080282c | 93 | float x = round(_msg.linear.x*100.0)/100.0; |
howanglam3 | 3:5402a080282c | 94 | float y = round(_msg.linear.y*100.0)/100.0; |
howanglam3 | 1:782534ae7166 | 95 | sum =abs(x)+abs(y); |
howanglam3 | 1:782534ae7166 | 96 | resize(y, sum); |
howanglam3 | 3:5402a080282c | 97 | if (x <= 0){ |
howanglam3 | 3:5402a080282c | 98 | if (y<=0){ |
howanglam3 | 3:5402a080282c | 99 | motor_pwm[0]= 0; |
howanglam3 | 3:5402a080282c | 100 | motor_pwm[1]= abs(y); |
howanglam3 | 3:5402a080282c | 101 | motor_pwm[2]= 0; |
howanglam3 | 3:5402a080282c | 102 | motor_pwm[3]= sum; |
howanglam3 | 3:5402a080282c | 103 | //turn(y, sum); |
howanglam3 | 3:5402a080282c | 104 | } |
howanglam3 | 3:5402a080282c | 105 | else{ |
howanglam3 | 3:5402a080282c | 106 | motor_pwm[0]= sum; |
howanglam3 | 3:5402a080282c | 107 | motor_pwm[1]= 0; |
howanglam3 | 3:5402a080282c | 108 | motor_pwm[2]= y; |
howanglam3 | 3:5402a080282c | 109 | motor_pwm[3]= 0; |
howanglam3 | 3:5402a080282c | 110 | } |
howanglam3 | 3:5402a080282c | 111 | } |
howanglam3 | 3:5402a080282c | 112 | else{ |
howanglam3 | 3:5402a080282c | 113 | if (y<=0){ |
howanglam3 | 3:5402a080282c | 114 | motor_pwm[0]= 0; |
howanglam3 | 3:5402a080282c | 115 | motor_pwm[1]= sum; |
howanglam3 | 3:5402a080282c | 116 | motor_pwm[2]= 0; |
howanglam3 | 3:5402a080282c | 117 | motor_pwm[3]= abs(y); |
howanglam3 | 3:5402a080282c | 118 | } |
howanglam3 | 3:5402a080282c | 119 | else{ |
howanglam3 | 3:5402a080282c | 120 | motor_pwm[0]= y; |
howanglam3 | 3:5402a080282c | 121 | motor_pwm[1]= 0; |
howanglam3 | 3:5402a080282c | 122 | motor_pwm[2]= sum; |
howanglam3 | 3:5402a080282c | 123 | motor_pwm[3]= 0; |
howanglam3 | 3:5402a080282c | 124 | } |
howanglam3 | 3:5402a080282c | 125 | } |
howanglam3 | 3:5402a080282c | 126 | // turn(sum, y); |
howanglam3 | 1:782534ae7166 | 127 | |
howanglam3 | 1:782534ae7166 | 128 | // sprintf(buffer, "turn: %f, %f", sum, y); |
howanglam3 | 1:782534ae7166 | 129 | // motor_msg.data = buffer; |
howanglam3 | 1:782534ae7166 | 130 | // motor_pub.publish(&motor_msg); |
howanglam3 | 0:54452c8078db | 131 | |
howanglam3 | 1:782534ae7166 | 132 | // |
howanglam3 | 1:782534ae7166 | 133 | // if (x==0.0){ |
howanglam3 | 1:782534ae7166 | 134 | // turn(y,y); |
howanglam3 | 0:54452c8078db | 135 | // } |
howanglam3 | 1:782534ae7166 | 136 | // else{ |
howanglam3 | 1:782534ae7166 | 137 | // float angle = atan(abs(y)/abs(x)); |
howanglam3 | 1:782534ae7166 | 138 | // float angle45 = atan(1.0); |
howanglam3 | 1:782534ae7166 | 139 | // if(x<0){ |
howanglam3 | 1:782534ae7166 | 140 | // if(angle <= angle45){ |
howanglam3 | 1:782534ae7166 | 141 | // motor_pwm[0]=0; |
howanglam3 | 1:782534ae7166 | 142 | // motor_pwm[1]=abs(x + abs(y)); |
howanglam3 | 1:782534ae7166 | 143 | // motor_pwm[2]=-x; |
howanglam3 | 1:782534ae7166 | 144 | // motor_pwm[3]=0; |
howanglam3 | 1:782534ae7166 | 145 | // |
howanglam3 | 1:782534ae7166 | 146 | // } |
howanglam3 | 1:782534ae7166 | 147 | // else{ |
howanglam3 | 1:782534ae7166 | 148 | // if(y<0){ |
howanglam3 | 1:782534ae7166 | 149 | // antiturning(-y, -y+x);} |
howanglam3 | 1:782534ae7166 | 150 | // else{ |
howanglam3 | 1:782534ae7166 | 151 | // turning(y+x, y);} |
howanglam3 | 1:782534ae7166 | 152 | // } |
howanglam3 | 1:782534ae7166 | 153 | // } |
howanglam3 | 1:782534ae7166 | 154 | // else { |
howanglam3 | 1:782534ae7166 | 155 | // if(angle <= angle45){ |
howanglam3 | 1:782534ae7166 | 156 | // motor_pwm[0]=x; |
howanglam3 | 1:782534ae7166 | 157 | // motor_pwm[1]=0; |
howanglam3 | 1:782534ae7166 | 158 | // motor_pwm[2]=0; |
howanglam3 | 1:782534ae7166 | 159 | // motor_pwm[3]=x - abs(y); |
howanglam3 | 1:782534ae7166 | 160 | // |
howanglam3 | 1:782534ae7166 | 161 | // } |
howanglam3 | 1:782534ae7166 | 162 | // else{ |
howanglam3 | 1:782534ae7166 | 163 | // if(y<0){ |
howanglam3 | 1:782534ae7166 | 164 | // antiturning(-y-x,-y);} |
howanglam3 | 1:782534ae7166 | 165 | // else{ |
howanglam3 | 1:782534ae7166 | 166 | // turning(y, y-x);} |
howanglam3 | 1:782534ae7166 | 167 | // } |
howanglam3 | 1:782534ae7166 | 168 | // } |
howanglam3 | 0:54452c8078db | 169 | // } |
howanglam3 | 0:54452c8078db | 170 | |
howanglam3 | 0:54452c8078db | 171 | } |
howanglam3 | 1:782534ae7166 | 172 | std_msgs::String str_msg; |
howanglam3 | 1:782534ae7166 | 173 | Publisher pub("battery", &str_msg); |
howanglam3 | 0:54452c8078db | 174 | |
howanglam3 | 0:54452c8078db | 175 | Subscriber<Twist> subtw("base_twist", &twCallback); |
howanglam3 | 0:54452c8078db | 176 | Subscriber<Bool> subr1("bt_r1", &r1Callback); |
howanglam3 | 1:782534ae7166 | 177 | |
howanglam3 | 1:782534ae7166 | 178 | void battery_pub(){ |
howanglam3 | 1:782534ae7166 | 179 | char buf[10]; |
howanglam3 | 1:782534ae7166 | 180 | while(1){ |
howanglam3 | 1:782534ae7166 | 181 | |
howanglam3 | 1:782534ae7166 | 182 | sprintf(buf, "%d", int(((battery.read()*3.3f) - (6/2.68))/(2.4/268))); |
howanglam3 | 1:782534ae7166 | 183 | // sprintf(buf,"%f",battery.read()); |
howanglam3 | 1:782534ae7166 | 184 | str_msg.data = buf; |
howanglam3 | 1:782534ae7166 | 185 | pub.publish(&str_msg); |
howanglam3 | 1:782534ae7166 | 186 | // nh.spinOnce(); |
howanglam3 | 1:782534ae7166 | 187 | Thread::wait(1000); |
howanglam3 | 1:782534ae7166 | 188 | |
howanglam3 | 1:782534ae7166 | 189 | } |
howanglam3 | 1:782534ae7166 | 190 | } |
howanglam3 | 1:782534ae7166 | 191 | |
howanglam3 | 0:54452c8078db | 192 | int main() { |
howanglam3 | 3:5402a080282c | 193 | // ros::Rate rate(10); |
howanglam3 | 3:5402a080282c | 194 | motor_pwm[0]=0; |
howanglam3 | 3:5402a080282c | 195 | motor_pwm[1]=0; |
howanglam3 | 3:5402a080282c | 196 | motor_pwm[2]=0; |
howanglam3 | 3:5402a080282c | 197 | motor_pwm[3]=0; |
howanglam3 | 0:54452c8078db | 198 | nh.getHardware()->setBaud(BAUD); |
howanglam3 | 0:54452c8078db | 199 | nh.initNode(); |
howanglam3 | 1:782534ae7166 | 200 | nh.advertise(pub); |
howanglam3 | 1:782534ae7166 | 201 | // nh.advertise(motor_pub); |
howanglam3 | 0:54452c8078db | 202 | nh.subscribe(subtw); |
howanglam3 | 0:54452c8078db | 203 | nh.subscribe(subr1); |
howanglam3 | 0:54452c8078db | 204 | |
howanglam3 | 1:782534ae7166 | 205 | battery_thread.start(battery_pub); |
howanglam3 | 0:54452c8078db | 206 | while (1) { |
howanglam3 | 0:54452c8078db | 207 | nh.spinOnce(); |
howanglam3 | 0:54452c8078db | 208 | |
howanglam3 | 0:54452c8078db | 209 | |
howanglam3 | 0:54452c8078db | 210 | if(nh.connected()) |
howanglam3 | 0:54452c8078db | 211 | myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson |
howanglam3 | 0:54452c8078db | 212 | else |
howanglam3 | 0:54452c8078db | 213 | myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson |
howanglam3 | 0:54452c8078db | 214 | } |
howanglam3 | 3:5402a080282c | 215 | Thread::wait(10); |
howanglam3 | 0:54452c8078db | 216 | } |