John Lam / Mbed 2 deprecated fyp

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?

UserRevisionLine numberNew 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 }