diff --git a/Drive/Arm_Drive_uart/Arm_drive_uart.ino b/Drive/Arm_Drive_uart/Arm_drive_uart.ino new file mode 100644 index 0000000..55d3c1d --- /dev/null +++ b/Drive/Arm_Drive_uart/Arm_drive_uart.ino @@ -0,0 +1,178 @@ +#include +#include +#include +#define baudrate 115200 + +ros::NodeHandle Uart_vel_command; //Node decleration +Controls_msgs::Rover Rover_obj; +//defining variables for recieved msg here +int right_vel; +int left_vel ; +bool left_bev_dir +bool left_bev_pwm +bool right_bev_dir +bool right_bev_pwm +bool l1_dir +bool l1_pwm +bool l2_dir +bool l2_pwm +bool base_dir +bool base_pwm +bool stepper_dir +bool stepper_step + +//declaring seiral pins and cyttron serial +int rxPin =0; +int txPin = 1; +float vel_wheels[6]; +int serial_pins[3] = {8,9,10}; + +#define POT1 A3 +#define POT2 A4 + +//1 is for 6 inch linear actuator, 2 is the 4 inch linear actuator, 3 is for the base rotator +//DIR1, PWM1, DIR2, PWM2, DIR3, PWM3 +int pin[6] = {7,13,6,12,5,11}; +//pot is the potentiometer reading. l is the stoke length(extension only). vel is the stoke velocity +float pot1,l1,vel1; +float pot2,l2,vel2; +float vel3; +//x,y is the stoke length that has to be reached. we get it from the final_ext topic. k is the right joystick input for the base rotation +float x,y,k; + +const int dirPin = 2; +const int stepPin = 3; +const int stepsPerRevolution = 100; +const int left_bev_pwm_pin = 8; +const int right_bev_pwm_pin = 9; +const int left_bev_dir_pin = 24; +const int right_bev_dir_pin = 26; + + +const int left_bevel_pwm = 80; +const int right_bevel_pwm = 80; +const int k =10; +const int l1 = 293.39 * (10**-3); +const int l2 = 240.49 * (10**-3); + +float arr[2] = { } + +SoftwareSerial MDDS1Serial=SoftwareSerial(rxPin,serial_pins[0]); +SoftwareSerial MDDS2Serial=SoftwareSerial(rxPin,serial_pins[1]); +SoftwareSerial MDDS3Serial=SoftwareSerial(rxPin,serial_pins[2]); + +//callback for subscriber +void sub_cb( const controls_msgs::Rover &Rover_obj){ + right_vel = Rover_obj.right_wheel_vel ; + left_vel = Rover_obj.left_wheel_vel ; + left_bev_dir = Rover_obj.left_bev_dir ; + if(Rover_obj.left_bev_pwm == true) + left_bev_pwm = 80 ; + else + left_bev_pwm = 0 ; + right_bev_dir = Rover_obj.right_bev_dir ; + if(Rover_obj.right_bev_pwm == true) + right_bev_pwm = 80 ; + else + right_bev_pwm = 0 ; + l1_dir = Rover_obj.l1_dir ; + if(Rover_obj.l1_pwm == true) + l1_pwm = k ; + else + l1_pwm = 0 ; + l2_dir = Rover_obj.l2_dir ; + if(Rover_obj.l2_pwm == true) + l2_pwm = k ; + else + l2_pwm = 0 ; + base_dir = Rover_obj.base_dir ; + if(Rover_obj.base_pwm == true) + base_pwm = 200 ; + else + base_pwm = 0 ; + stepper_dir = Rover_obj.stepper_dir ; + stepper_step = Rover_obj.stepper_step ; + +} + + +void setup() { + + Uart_vel_command.initNode(); + Uart_vel_command.subscribe(Rover_values_sub); + ros::Publisher chatter_pub = n.advertise("chatter", 1000); + //publisher + + for ( int i=0; i<3 ;i++) + { + pinMode(serial_pins[i],OUTPUT); + } + + MDDS1Serial.begin(baudrate); + MDDS2Serial.begin(baudrate); + MDDS3Serial.begin(baudrate); + + delay(1000); + int a = 0 ; + int b = 128 ; + MDDS1Serial.write(a); + MDDS1Serial.write(b); + + MDDS2Serial.write(a); + MDDS2Serial.write(b); + + MDDS3Serial.write(a); + MDDS3Serial.write(b); + delay(100); + + pinMode(stepPin, OUTPUT); + pinMode(dirPin, OUTPUT); + for(int i=0; i<6; i++){ + pinMode(pin[i],OUTPUT); + } + + } + +void loop() +{ + ros::SubscriberRover_values_sub("/To_Arduino", &sub_cb); + //Subscriber + + + MDDS1Serial.write(right_wheel_vel); + MDDS1Serial.write(left_wheel_vel); + + MDDS2Serial.write(right_wheel_vel); + MDDS2Serial.write(left_wheel_vel); + + MDDS3Serial.write(right_wheel_vel); + MDDS3Serial.write(left_wheel_vel); + Uart_vel_command.spinOnce(); + + + digitalWrite(pin[4], base_dir); + analogWrite(pin[5], base_pwm); + digitalWrite(left_bev_dir_pin, left_bev_dir); // verify + analogWrite(left_bev_pwm_pin, left_bev_pwm); + digitalWrite(right_bev_dir_pin, right_bev_pwm); // verify + analogWrite(right_bev_pwm_pin, right_bevl_pwm); + digitalWrite(pin[0], l1_dir); + analogWrite(pin[1], l1_pwm); + digitalWrite(pin[2], l2_dir); // verify + analogWrite(pin[3], l3_pwm); + digitalWrite(dirPin, stepper_dir); + digitalWrite(stepPin, stepper_step); + delay(200); + int f1 = analogRead(A3); + int f2 = analogRead(A4); // pwm from potentiometer + float s1= (f1/255)*l1; // feedback stroke length + float s2= (f2/255)*l2; + std_msgs::Float64MultiArray array_msg; + array_msg.data.resize(2); + array_msg.data[1]=s1 + array_msg.data[2]=s2 + chatter_pub.publish(array_msg) + + + +} diff --git a/Drive/msg/rover.msg b/Drive/msg/rover.msg new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Drive/msg/rover.msg @@ -0,0 +1 @@ + diff --git a/Drive/uart_drive/uart_drive.ino b/Drive/uart_drive/uart_drive.ino deleted file mode 100644 index 5646c5d..0000000 --- a/Drive/uart_drive/uart_drive.ino +++ /dev/null @@ -1,126 +0,0 @@ - -#include -#define baudrate 115200 -#define vmax 0.22 -#define wmax 2.84 -int rxPin =0; -int txPin = 1; -float v; -float omega; -float vel_wheels[6]; -int serial_pins[3] = {8,9,10}; -float left_wheel,right_wheel; -signed int motorLspeed1,motorLspeed2,motorLspeed3,motorRspeed1,motorRspeed2,motorRspeed3; -uint8_t commandbyte; -SoftwareSerial myserial = SoftwareSerial(rxPin,txPin); -SoftwareSerial MDDS1Serial = SoftwareSerial(rxPin,serial_pins[0]); -SoftwareSerial MDDS2Serial = SoftwareSerial(rxPin,serial_pins[1]); -SoftwareSerial MDDS3Serial = SoftwareSerial(rxPin,serial_pins[2]); - -float mymap(float c,float a,float b,float d,float e) -{ - return d+(c-a)*(e-d)/(b-a); - } - -void setup() { - - myserial.begin(baudrate); - delay(1000); - for ( int i=0; i<3 ;i++) - { - pinMode(serial_pins[i],OUTPUT); - } - - MDDS1Serial.begin(baudrate); - MDDS2Serial.begin(baudrate); - MDDS3Serial.begin(baudrate); - delay(1000); - - } - -void loop() -{ - //hardware serial for getting velocity from jetson to arduinomega - if(myserial.available()>0) - { - left_wheel = myserial.read(); - right_wheel = myserial.read(); - } - for (int i =0;i<3;i++) - { vel_wheels[i] = left_wheel; - } - for (int i =5;i>=3;i--) - { vel_wheels[i] = right_wheel; - - } - - //for cytron 1 - motorLspeed1 = vel_wheels[0]; - motorRspeed1 = vel_wheels[3]; - - if(motorLspeed1>=0) - commandbyte = 0; - else if(motorLspeed1 <0) - commandbyte =0x40; - - commandbyte = commandbyte | motorLspeed1; - MDDS1Serial.write(commandbyte); - - if(motorRspeed1>=0) - commandbyte = 0xC0; - else if(motorRspeed1<0) - commandbyte = 0x80; - - commandbyte = commandbyte | motorRspeed1; - MDDS1Serial.write(commandbyte); - - Serial.println(commandbyte); - - //for cytron 2 - motorLspeed2 = vel_wheels[1]; - motorRspeed2 = vel_wheels[4]; - - if(motorLspeed2>=0) - commandbyte = 0; - else if(motorLspeed2 <0) - commandbyte =0x40; - - commandbyte = commandbyte | motorLspeed2; - MDDS2Serial.write(commandbyte); - - if(motorRspeed2>=0) - commandbyte = 0xC0; - else if(motorRspeed2<0) - commandbyte = 0x80; - - commandbyte = commandbyte | motorRspeed2; - MDDS2Serial.write(commandbyte); - - Serial.println(commandbyte); - //cytron 3 - - motorLspeed3 = vel_wheels[2]; - motorRspeed3 = vel_wheels[5]; - - if(motorLspeed3>=0) - commandbyte = 0; - else if(motorLspeed3 <0) - commandbyte =0x40; - - commandbyte = commandbyte | motorLspeed1; - MDDS3Serial.write(commandbyte); - - if(motorRspeed3>=0) - commandbyte = 0xC0; - else if(motorRspeed3<0) - commandbyte = 0x80; - - commandbyte = commandbyte | motorRspeed3; - MDDS3Serial.write(commandbyte); - - Serial.println(commandbyte); - - - delay(200); - -} diff --git a/Drive/uart_drive/uart_drive_rosserial.ino b/Drive/uart_drive/uart_drive_rosserial.ino deleted file mode 100644 index dc97ce0..0000000 --- a/Drive/uart_drive/uart_drive_rosserial.ino +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include -#include -#include -#define baudrate 115200 -#define vmax 0.22 -#define wmax 2.84 - -ros::NodeHandle nh; //Node decleration - -geometry_msgs::Twist twist_obj; -geometry_msgs::Twist pwms_obj; - -float v; //recieved Lin Vel from cmd_vel -float omega; //recieved Ang vel from cmd_vel -float ang_vel; //omega mapped down to ang_vel - - -int rxPin =0; -int txPin = 1; -float vel_wheels[6]; -int serial_pins[3] = {8,9,10}; -float motorLspeed1,motorLspeed2,motorLspeed3,motorRspeed1,motorRspeed2,motorRspeed3; - - -SoftwareSerial MDDS1Serial=SoftwareSerial(rxPin,serial_pins[0]); -SoftwareSerial MDDS2Serial=SoftwareSerial(rxPin,serial_pins[1]); -SoftwareSerial MDDS3Serial=SoftwareSerial(rxPin,serial_pins[2]); - -void sub_cb(const geometry_msgs::Twist &twist_obj) -{ - ang_vel = twist_obj.angular.z; - omega = mymap(ang_vel , -2.84, 2.84, -0.5, 0.5); - //if (ang_vel <= 0.05 && ang_vel >= -0.05) omega = 0; - //else omega = map(ang_vel, -2.84, 2.84, -0.5, 0.5); - v = twist_obj.linear.x; -} - -ros::Subscriber cmd_vel_sub("/cmd_vel", &sub_cb); -ros::Publisher chatter("chatter", &twist_obj); -ros::Publisher pwms_pub("/pwms", &pwms_obj); - -float mymap(float c,float a,float b,float d,float e) -{ - return d+(c-a)*(e-d)/(b-a); - } - -void setup() { - - nh.initNode(); - nh.subscribe(cmd_vel_sub); - nh.advertise(chatter); - - for ( int i=0; i<3 ;i++) - { - pinMode(serial_pins[i],OUTPUT); - } - - MDDS1Serial.begin(baudrate); - MDDS2Serial.begin(baudrate); - MDDS3Serial.begin(baudrate); - - delay(1000); - - MDDS1Serial.write(0x80); - MDDS2Serial.write(0x80); - MDDS3Serial.write(0x80); - delay(100); - } - -void loop() -{ - //right wheels - vel_wheels[0] = v + omega; //back - vel_wheels[1] = v + omega; //middle - vel_wheels[2] = v + omega; //front - - // For left wheels - vel_wheels[3] = v - omega; //back - vel_wheels[4] = v - omega; //middle - vel_wheels[5] = v - omega; //front - - twist_obj.linear.x = vel_wheels[0]; - twist_obj.linear.y = vel_wheels[1]; - twist_obj.linear.z = vel_wheels[2]; - twist_obj.angular.x = vel_wheels[3]; - twist_obj.angular.y = vel_wheels[4]; - twist_obj.angular.z = vel_wheels[5]; - - //hardware serial for getting velocity from jetson to arduinomega - //for cytron 1 - motorLspeed1 = vel_wheels[3]; - motorRspeed1 = vel_wheels[0]; - - if(motorLspeed1>=0) - MDDS1Serial.write(motorLspeed1); - if(motorRspeed1>=0); - MDDS1Serial.write(motorRspeed1); - - //for cytron 2 - motorLspeed2 = vel_wheels[1]; - motorRspeed2 = vel_wheels[4]; - - if(motorLspeed2>=0) - MDDS2Serial.write(motorLspeed2); - if(motorRspeed2>=0); - MDDS2Serial.write(motorRspeed2); - - //cytron 3 - motorLspeed3 = vel_wheels[5]; - motorRspeed3 = vel_wheels[2]; - - if(motorLspeed3>=0) - MDDS3Serial.write(motorLspeed3); - if(motorRspeed3>=0); - MDDS3Serial.write(motorRspeed3); - - chatter.publish(&twist_obj); - nh.spinOnce(); - - delay(200); - -} diff --git a/Drive/uart_drive/wheel_vel.py b/Drive/uart_drive/wheel_vel.py deleted file mode 100644 index 3306661..0000000 --- a/Drive/uart_drive/wheel_vel.py +++ /dev/null @@ -1,56 +0,0 @@ -import rospy -import serial -from geometry_msgs.msg import Twist - -vmax ="max velocity " -omegamax ="max omega" - -class drive(): - def __init__(self): - self.vel_sub = rospy.Subscriber("cmd_vel",Twist,self.cmd_cb) - self.rate = rospy.Rate(100); - self.my_serial = serial.Serial('/dev/tty/ACMO',115200,timeout=1) - - def cmd_cb(self,data): - self.v = data.linear.x - self.omega = data.angular.z - - right_wheel =self.v + self.omega - left_wheel = self.v - self.omega - - if (left_wheel > 0): - left_speed = self.mymap(left_wheel, 0, VMAX+OMEGAMAX, 0, 63) - else: - left_speed = self.mymap(-left_wheel, 0, VMAX+OMEGAMAX, 65, 127) - - if (right_wheel > 0): - right_speed = self.mymap(right_wheel, 0, VMAX+OMEGAMAX, 129, 191) - else: - right_speed = self.mymap(-right_wheel, 0, VMAX+OMEGAMAX, 193, 255) - - if (self.v == 0 and self.omega == 0): - right_wheel = 128 - left_wheel = 64 - - # first send command for left wheels - command = str(left_speed) - command = int(bin(int(command)).replace("0b","")) - - self.my_serial.write(command) - - # right wheels - command = str(right_speed) - command = int(bin(int(command)).replace("0b","")) - - self.my_serial.write(command) - - self.rate.sleep() - - def mymap(self,c,a,b,d,e): - return d + (c-a)*(e-d)/(b-a) - - -if __name__ == "__main__": - rospy.init_node("motor_controller") - drive() - rospy.spin() \ No newline at end of file