├── 4WD_encoder_ros_arduino └── 4WD_encoder_ros_arduino.ino └── teleop /4WD_encoder_ros_arduino/4WD_encoder_ros_arduino.ino: -------------------------------------------------------------------------------- 1 | #define USE_USBCON 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // Initialize PID paramaters 10 | 11 | double Setpoint_fl, Input_fl, Output_fl; 12 | double Setpoint_fr, Input_fr, Output_fr; 13 | double Setpoint_bl, Input_bl, Output_bl; 14 | double Setpoint_br, Input_br, Output_br; 15 | 16 | double aggKp=450, aggKi=250, aggKd=4; 17 | double consKp=280, consKi=120, consKd=1; 18 | 19 | PID myPID_fl(&Input_fl, &Output_fl, &Setpoint_fl, aggKp, aggKi, aggKd, DIRECT); 20 | PID myPID_fr(&Input_fr, &Output_fr, &Setpoint_fr, aggKp, aggKi, aggKd, DIRECT); 21 | PID myPID_bl(&Input_bl, &Output_bl, &Setpoint_bl, aggKp, aggKi, aggKd, DIRECT); 22 | PID myPID_br(&Input_br, &Output_br, &Setpoint_br, aggKp, aggKi, aggKd, DIRECT); 23 | 24 | // Initialize quadrature encoder paramaters 25 | 26 | Quadrature_encoder<46,47> encoder_fright(Board::due); 27 | Quadrature_encoder<48,49> encoder_fleft(Board::due); 28 | Quadrature_encoder<42,43> encoder_bright(Board::due); 29 | Quadrature_encoder<44,45> encoder_bleft(Board::due); 30 | 31 | // Initialize pin numbers 32 | 33 | const uint8_t RF_PWM = 11; 34 | const uint8_t RF_BACK = 27; 35 | const uint8_t RF_FORW = 26; 36 | const uint8_t LF_BACK = 24; 37 | const uint8_t LF_FORW = 25; 38 | const uint8_t LF_PWM = 10; 39 | 40 | const uint8_t RB_PWM = 13; 41 | const uint8_t RB_BACK = 30; 42 | const uint8_t RB_FORW = 31; 43 | const uint8_t LB_BACK = 29; 44 | const uint8_t LB_FORW = 28; 45 | const uint8_t LB_PWM = 12; 46 | 47 | // speed = 0? help? pid not reset? 48 | bool wtf; 49 | int ticks_since_target = 0; 50 | 51 | 52 | // Initialize ROS paramaters 53 | 54 | ros::NodeHandle nh; 55 | 56 | std_msgs::Int32 lfcount; 57 | std_msgs::Int32 rfcount; 58 | std_msgs::Int32 lbcount; 59 | std_msgs::Int32 rbcount; 60 | 61 | ros::Publisher lfwheel("lfwheel", &lfcount); 62 | ros::Publisher rfwheel("rfwheel", &rfcount); 63 | ros::Publisher lbwheel("lbwheel", &lbcount); 64 | ros::Publisher rbwheel("rbwheel", &rbcount); 65 | 66 | // Cmd_vel Callback 67 | // Sets the setpoints of the pid for each wheel 68 | 69 | void onTwist(const geometry_msgs::Twist &msg) 70 | { 71 | //nh.loginfo("Inside Callback"); 72 | float x = msg.linear.x; 73 | float z = msg.angular.z; 74 | if(x > 0.3){x = 0.3;} 75 | if(z > 0.25){z = 0.2;} 76 | float w = 0.2; 77 | if(!(x==0 && z==0)){ 78 | wtf=false; 79 | Setpoint_fr = x + (z * w / 2.0)/0.1; 80 | Setpoint_fl = x - (z * w / 2.0)/0.1; 81 | Setpoint_br = x + (z * w / 2.0)/0.1; 82 | Setpoint_bl = x - (z * w / 2.0)/0.1; 83 | } 84 | else{ 85 | wtf=true; 86 | Setpoint_fl = 0; 87 | Setpoint_fr = 0; 88 | Setpoint_bl = 0; 89 | Setpoint_br = 0; 90 | 91 | } 92 | 93 | } 94 | 95 | ros::Subscriber sub("cmd_vel", &onTwist); 96 | 97 | // Move any motor function with speed_pwm value and pin numbers 98 | 99 | void Move_motor(int speed_pwm,const uint8_t pwm,const uint8_t forw,const uint8_t back) 100 | { 101 | if(speed_pwm >= 0) 102 | { 103 | digitalWrite(forw, HIGH); 104 | digitalWrite(back, LOW); 105 | analogWrite(pwm, abs(speed_pwm)); 106 | } 107 | else if(speed_pwm < 0) 108 | { 109 | digitalWrite(forw, LOW); 110 | digitalWrite(back, HIGH); 111 | analogWrite(pwm, abs(speed_pwm)); 112 | } 113 | } 114 | 115 | 116 | 117 | // Initialize pins for forward movement 118 | 119 | void setpins() 120 | { 121 | pinMode(LF_FORW,OUTPUT); 122 | pinMode(LF_BACK,OUTPUT); 123 | pinMode(RF_FORW,OUTPUT); 124 | pinMode(RF_BACK,OUTPUT); 125 | pinMode(LF_PWM,OUTPUT); 126 | pinMode(RF_PWM,OUTPUT); 127 | pinMode(LB_FORW,OUTPUT); 128 | pinMode(LB_BACK,OUTPUT); 129 | pinMode(RB_FORW,OUTPUT); 130 | pinMode(RB_BACK,OUTPUT); 131 | pinMode(LB_PWM,OUTPUT); 132 | pinMode(RB_PWM,OUTPUT); 133 | 134 | digitalWrite(RF_FORW, HIGH); 135 | digitalWrite(RF_BACK, LOW); 136 | digitalWrite(LF_FORW, HIGH); 137 | digitalWrite(LF_BACK, LOW); 138 | digitalWrite(RB_FORW, HIGH); 139 | digitalWrite(RB_BACK, LOW); 140 | digitalWrite(LB_FORW, HIGH); 141 | digitalWrite(LB_BACK, LOW); 142 | } 143 | 144 | // Encoders tend to reverse regarding of the pins?? 145 | // This way we move the robot forward a bit on startup 146 | // And if an encoder has negative value we reverse it. 147 | 148 | void fix_encoder_ori_on_start(){ 149 | 150 | analogWrite(RF_PWM, 180); 151 | analogWrite(LF_PWM, 180); 152 | analogWrite(RB_PWM, 180); 153 | analogWrite(LB_PWM, 180); 154 | 155 | delay(150); 156 | 157 | analogWrite(RF_PWM, 0); 158 | analogWrite(LF_PWM, 0); 159 | analogWrite(RB_PWM, 0); 160 | analogWrite(LB_PWM, 0); 161 | 162 | int ct1 = encoder_fleft.count(); 163 | int ct2 = encoder_fright.count(); 164 | int ct3 = encoder_bleft.count(); 165 | int ct4 = encoder_bright.count(); 166 | if(ct1 < 0) {encoder_fleft.reverse();} 167 | if(ct2 < 0) {encoder_fright.reverse();} 168 | if(ct3 < 0) {encoder_bleft.reverse();} 169 | if(ct4 < 0) {encoder_bright.reverse();} 170 | 171 | } 172 | 173 | //void reset Integral error when we stop 174 | void reset_pid_Ki() 175 | { 176 | myPID_fl.SetMode(MANUAL); 177 | myPID_fr.SetMode(MANUAL); 178 | myPID_bl.SetMode(MANUAL); 179 | myPID_br.SetMode(MANUAL); 180 | Output_fl=0; 181 | Output_fr=0; 182 | Output_bl=0; 183 | Output_br=0; 184 | //myPID_fl.SetTunings(aggKp, 0, aggKd); 185 | //myPID_fr.SetTunings(aggKp, 0, aggKd); 186 | //myPID_bl.SetTunings(aggKp, 0, aggKd); 187 | //myPID_br.SetTunings(aggKp, 0, aggKd); 188 | myPID_fl.SetMode(AUTOMATIC); 189 | myPID_fr.SetMode(AUTOMATIC); 190 | myPID_bl.SetMode(AUTOMATIC); 191 | myPID_br.SetMode(AUTOMATIC); 192 | //myPID_fl.SetTunings(aggKp, aggKi, aggKd); 193 | //myPID_fr.SetTunings(aggKp, aggKi, aggKd); 194 | //myPID_bl.SetTunings(aggKp, aggKi, aggKd); 195 | //myPID_br.SetTunings(aggKp, aggKi, aggKd); 196 | } 197 | 198 | // stop movement 199 | 200 | void stop() 201 | { 202 | digitalWrite(LF_FORW, 0); 203 | digitalWrite(LF_BACK, 0); 204 | digitalWrite(RF_FORW, 0); 205 | digitalWrite(RF_BACK, 0); 206 | analogWrite(LF_PWM, 0); 207 | analogWrite(RF_PWM, 0); 208 | digitalWrite(LB_FORW, 0); 209 | digitalWrite(LB_BACK, 0); 210 | digitalWrite(RB_FORW, 0); 211 | digitalWrite(RB_BACK, 0); 212 | analogWrite(LB_PWM, 0); 213 | analogWrite(RB_PWM, 0); 214 | 215 | } 216 | 217 | void setup() { 218 | 219 | // 115200 baud rate 220 | nh.getHardware()->setBaud(115200); 221 | 222 | // Pid setup 223 | 224 | myPID_fl.SetOutputLimits(-255, 255); 225 | myPID_fr.SetOutputLimits(-255, 255); 226 | myPID_bl.SetOutputLimits(-255, 255); 227 | myPID_br.SetOutputLimits(-255, 255); 228 | 229 | myPID_fl.SetMode(AUTOMATIC); 230 | myPID_fr.SetMode(AUTOMATIC); 231 | myPID_bl.SetMode(AUTOMATIC); 232 | myPID_br.SetMode(AUTOMATIC); 233 | 234 | myPID_fl.SetSampleTime(20); 235 | myPID_fr.SetSampleTime(20); 236 | myPID_bl.SetSampleTime(20); 237 | myPID_br.SetSampleTime(20); 238 | 239 | // Encoder setup 240 | 241 | encoder_fright.begin(); 242 | encoder_fleft.begin(); 243 | encoder_bright.begin(); 244 | encoder_bleft.begin(); 245 | 246 | // setup pins and fix encoders 247 | 248 | setpins(); 249 | fix_encoder_ori_on_start(); 250 | 251 | stop(); 252 | 253 | // ros node setup 254 | 255 | nh.initNode(); 256 | nh.advertise(lfwheel); 257 | nh.advertise(rfwheel); 258 | nh.advertise(lbwheel); 259 | nh.advertise(rbwheel); 260 | nh.subscribe(sub); 261 | 262 | } 263 | 264 | // Initialize starting loop paramaters for calculating velocity and time 265 | 266 | unsigned long prev = 0; 267 | int old_ct1=0; 268 | int old_ct2=0; 269 | int old_ct3=0; 270 | int old_ct4=0; 271 | float ticks_per_meter = 33000.1; 272 | 273 | void loop() { 274 | 275 | // count encoder ticks 276 | int ct1 = encoder_fleft.count(); 277 | int ct2 = encoder_fright.count(); 278 | int ct3 = encoder_bleft.count(); 279 | int ct4 = encoder_bright.count(); 280 | // for some reason if i omit this it does not work properly 281 | if (ct1!=-1){ 282 | lfcount.data = ct1;} 283 | if (ct2!=-1){ 284 | rfcount.data = ct2;} 285 | if (ct3!=-1){ 286 | lbcount.data = ct3;} 287 | if (ct4!=-1){ 288 | rbcount.data = ct4;} 289 | // Publish encoder ticks to calculate odom on Jetson Nano side 290 | lfwheel.publish(&lfcount); 291 | rfwheel.publish(&rfcount); 292 | lbwheel.publish(&lbcount); 293 | rbwheel.publish(&rbcount); 294 | 295 | // calculate time and current velocity 296 | 297 | unsigned long now = millis(); 298 | Input_fl = (float(ct1 - old_ct1) / ticks_per_meter) / ((now - prev) / 1000.0); 299 | Input_fr = (float(ct2 - old_ct2) / ticks_per_meter) / ((now - prev) / 1000.0); 300 | Input_bl = (float(ct3 - old_ct3) / ticks_per_meter) / ((now - prev) / 1000.0); 301 | Input_br = (float(ct4 - old_ct4) / ticks_per_meter) / ((now - prev) / 1000.0); 302 | 303 | // Use aggresive pid paramaters if gap > 0.1 else conservative 304 | bool gap1 = abs(Setpoint_fl - Input_fl) < 0.12; 305 | bool gap2 = abs(Setpoint_fr - Input_fr) < 0.12; 306 | bool gap3 = abs(Setpoint_bl - Input_bl) < 0.12; 307 | bool gap4 = abs(Setpoint_br - Input_br) < 0.12; 308 | 309 | if(gap1 && gap2 && gap3 && gap4){ 310 | myPID_fl.SetTunings(consKp, consKi, consKd); 311 | myPID_fr.SetTunings(consKp, consKi, consKd); 312 | myPID_bl.SetTunings(consKp, consKi, consKd); 313 | myPID_br.SetTunings(consKp, consKi, consKd); 314 | 315 | } 316 | else{ 317 | myPID_fl.SetTunings(aggKp, aggKi, aggKd); 318 | myPID_fr.SetTunings(aggKp, aggKi, aggKd); 319 | myPID_bl.SetTunings(aggKp, aggKi, aggKd); 320 | myPID_br.SetTunings(aggKp, aggKi, aggKd); 321 | } 322 | if(wtf){ 323 | reset_pid_Ki(); 324 | } 325 | // Compute Pid 326 | myPID_fl.Compute(); 327 | myPID_fr.Compute(); 328 | myPID_bl.Compute(); 329 | myPID_br.Compute(); 330 | 331 | 332 | 333 | 334 | // Move the motors with the output of the pid 335 | 336 | Move_motor(Output_fl,LF_PWM,LF_FORW,LF_BACK); 337 | Move_motor(Output_fr,RF_PWM,RF_FORW,RF_BACK); 338 | Move_motor(Output_bl,LB_PWM,LB_FORW,LB_BACK); 339 | Move_motor(Output_br,RB_PWM,RB_FORW,RB_BACK); 340 | 341 | // spin the ros node 342 | 343 | nh.spinOnce(); 344 | // take the old encoder ticks and time for calculating velocity 345 | old_ct1 = encoder_fleft.count(); 346 | old_ct2 = encoder_fright.count(); 347 | old_ct3 = encoder_bleft.count(); 348 | old_ct4 = encoder_bright.count(); 349 | prev = now; 350 | 351 | delay(25); 352 | 353 | } 354 | -------------------------------------------------------------------------------- /teleop: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Willow Garage, Inc. nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | import rospy 31 | 32 | from geometry_msgs.msg import Twist 33 | 34 | import sys, select, termios, tty 35 | 36 | msg = """ 37 | Control Your Udacity Bot! 38 | --------------------------- 39 | Moving around: 40 | u i o 41 | j k l 42 | m , . 43 | 44 | q/z : increase/decrease max speeds by 10% 45 | w/x : increase/decrease only linear speed by 10% 46 | e/c : increase/decrease only angular speed by 10% 47 | space key, k : force stop 48 | anything else : stop smoothly 49 | 50 | CTRL-C to quit 51 | """ 52 | 53 | moveBindings = { 54 | 'i':(1,0), 55 | 'o':(1,-1), 56 | 'j':(0,1), 57 | 'l':(0,-1), 58 | 'u':(1,1), 59 | ',':(-1,0), 60 | '.':(-1,1), 61 | 'm':(-1,-1), 62 | } 63 | 64 | speedBindings={ 65 | 'q':(1.1,1.1), 66 | 'z':(.9,.9), 67 | 'w':(1.1,1), 68 | 'x':(.9,1), 69 | 'e':(1,1.1), 70 | 'c':(1,.9), 71 | } 72 | 73 | def getKey(): 74 | tty.setraw(sys.stdin.fileno()) 75 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 76 | if rlist: 77 | key = sys.stdin.read(1) 78 | else: 79 | key = '' 80 | 81 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 82 | return key 83 | 84 | speed = 0.2 85 | turn = 0.5 86 | 87 | def vels(speed,turn): 88 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 89 | 90 | if __name__=="__main__": 91 | settings = termios.tcgetattr(sys.stdin) 92 | 93 | rospy.init_node('teleop') 94 | pub = rospy.Publisher('~cmd_vel', Twist, queue_size=5) 95 | 96 | x = 0 97 | th = 0 98 | status = 0 99 | count = 0 100 | acc = 0.1 101 | target_speed = 0 102 | target_turn = 0 103 | control_speed = 0 104 | control_turn = 0 105 | try: 106 | print(msg) 107 | print(vels(speed,turn)) 108 | while(1): 109 | key = getKey() 110 | if key in moveBindings.keys(): 111 | x = moveBindings[key][0] 112 | th = moveBindings[key][1] 113 | count = 0 114 | elif key in speedBindings.keys(): 115 | speed = speed * speedBindings[key][0] 116 | turn = turn * speedBindings[key][1] 117 | count = 0 118 | 119 | print(vels(speed,turn)) 120 | if (status == 14): 121 | print(msg) 122 | status = (status + 1) % 15 123 | elif key == ' ' or key == 'k' : 124 | x = 0 125 | th = 0 126 | control_speed = 0 127 | control_turn = 0 128 | else: 129 | count = count + 1 130 | if count > 4: 131 | x = 0 132 | th = 0 133 | if (key == '\x03'): 134 | break 135 | 136 | target_speed = speed * x 137 | target_turn = turn * th 138 | 139 | if target_speed > control_speed: 140 | control_speed = min( target_speed, control_speed + 0.02 ) 141 | elif target_speed < control_speed: 142 | control_speed = max( target_speed, control_speed - 0.02 ) 143 | else: 144 | control_speed = target_speed 145 | 146 | if target_turn > control_turn: 147 | control_turn = min( target_turn, control_turn + 0.1 ) 148 | elif target_turn < control_turn: 149 | control_turn = max( target_turn, control_turn - 0.1 ) 150 | else: 151 | control_turn = target_turn 152 | 153 | twist = Twist() 154 | twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 155 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 156 | pub.publish(twist) 157 | 158 | except Exception as e: 159 | print(e) 160 | 161 | finally: 162 | twist = Twist() 163 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 164 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 165 | pub.publish(twist) 166 | 167 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 168 | 169 | --------------------------------------------------------------------------------