Robocon 2021
MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 8:9abe58bd7c07
- Parent:
- 6:0b79b90b83a2
--- a/main.cpp Wed Jun 16 10:10:57 2021 +0000
+++ b/main.cpp Fri Jun 18 16:34:51 2021 +0000
@@ -18,6 +18,26 @@
void itoa(int num, char *str, int radix);
+// ROS Connect Baud Rate
+#define BAUD_RATE 115200
+
+#include "mbed.h"
+#include "Servo.h"
+#include <ros.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int16MultiArray.h>
+#include "DC_Motor_Controller.h"
+
+
+// Declare Functions
+void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
+void msgCb_button(const std_msgs::Int8 &button_msg);
+void msgCam(const std_msgs::Int16 &cam_msg);
+
+void itoa(int num, char *str, int radix);
+
// Define ros variables
ros::NodeHandle nh;
// Subs
@@ -37,24 +57,30 @@
DigitalIn button(USER_BUTTON);
// LED
DigitalOut myled(LED1);
-// Faulhaber pins
-DigitalOut pin1 = A3; // pin 1 of faulhaber
-DigitalOut pin2 = A5; // pin 2 of faulhaber
-DigitalOut pin3 = A4; // pin 3 of faulhaber
-DigitalOut pin4 = A0; // pin 4 of faulhaber
+// Faulhaber1 pins
+DigitalOut pin1 = A3; // pin 1 of faulhaber1
+DigitalOut pin2 = A5; // pin 2 of faulhaber1
+DigitalOut pin3 = A4; // pin 3 of faulhaber1
+DigitalOut pin4 = A0; // pin 4 of faulhaber1
AnalogOut aout = D13;
+// Faulhaber2 pins
+DigitalOut faul2_1 = A1; // pin1 of faulhaber2
+DigitalOut faul2_2 = A2; // pin2 of faulhaber2
// Electromagnet
DigitalOut lock = D14;
// Firing pin
-DigitalIn firing1 = D9; // Reset Faulhaber
+DigitalIn firing1 = D2; // Reset Faulhaber
DigitalIn firing2 = D15; // Load 1 arrow
-DigitalIn firing3 = D2; // Pick, small
+DigitalIn firing3 = D7; // Pick, short
+DigitalIn firing4 = D4; // Pick, longError: Identifier "faul2_1" is undefined in "main.cpp", Line: 241, Col: 14
+DigitalIn firing5 = D8; // Pick, long - 2
+DigitalIn firing6 = D9;
// Valves
-DigitalOut eject_1 = D4; // valve 1
-DigitalOut eject_2 = D7; // valve 2
+DigitalOut valve = D3; // valve
+
// DC motors
DC_Motor_PID dc_motor1(D6, D5, PE_6, PF_8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
-DC_Motor_PID dc_motor2(D10, D11, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works? original: D4, D7 for encoder, D12, D13 for out
+DC_Motor_PID dc_motor2(D10, D12, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works? original: D4, D7 for encoder, D12, D13 for out
// Global Control variable
char msg[200] = {0}; // msg for responser
@@ -65,48 +91,49 @@
int load_manual = 1; // tune the loading 1 arrow manually
int auto_shooting = 0;
int smooth = 0;
-
+int auto_shooting_goToTargetPos = 0;
+int start = 0; // control variable for resetting shooter
+int touch = 0;
// cam callback
void msgCam(const std_msgs::Int16 &cam_msg)
{
- float minimum = 250;
- float maximum = 600;
- /*
- if( cam_msg.data > 0)
- {
- aout = (maximum - float(cam_msg.data)) / (maximum - minimum);
- if(auto_shooting)
- {
- memset(msg2, 0, sizeof(msg2));
- strcpy(msg2, "Cam distance: ");
- itoa(cam_msg.data, msg2 + strlen(msg2), 10);
- strcpy(msg2 + strlen(msg2), ", aout(1000): ");
- itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
- // sprintf("Cam distance: %d, %.4f", data, aout.read());
- responser2.publish(&str_msg2);
- }
- }*/
if( cam_msg.data > 0 && auto_shooting)
{
- int outValue[3] = [cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10];
- if(auto_shooting)
+
+ int outValue[3] = {cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10};
+ if(auto_shooting && auto_shooting_goToTargetPos)
{
- memset(msg2, 0, sizeof(msg2));
- strcpy(msg2, "Cam distance: ");
- itoa(cam_msg.data, msg2 + strlen(msg2), 10);
- strcpy(msg2 + strlen(msg2), ", aout(1000): ");
- itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
- // sprintf("Cam distance: %d, %.4f", data, aout.read());
- responser2.publish(&str_msg2);
+ pin1 = 0;
+ pin2 = 0;
+ pin3 = 1;
+ wait(0.2);
+ aout = 1;
+ pin3 = 1;
+ pin2 = 1;
+ pin1 = 1;
+ wait(0.1);
+ auto_shooting_goToTargetPos = false;
+
for(int i = 0; i<3; i++){
- outValue[i]/8;
- (outValue[i]/4)%2;
- (outValue[i]/2
- (outValue[i]%2;
+ //int val[4] = {outValue[i]/8, outValue[i]/4%2, outValue[i]/2%2, outValue[i]%2};
+ aout = outValue[i]/8;
+ pin3 = outValue[i]/4%2;
+ pin2= outValue[i]/2%2;
+ pin1 = outValue[i]%2;
+ wait(0.1);
+ aout = 1;
+ pin3 = 1;
+ pin2 = 1;
+ pin1 = 1;
+ wait(0.1);
}
+ aout = 0; pin3 = 0; pin2 = 0; pin1 = 0; pin4 = 0;
}
+ memset(msg2, 0, sizeof(msg2));
+ strcpy(msg2, "Cam distance: ");
+ itoa(cam_msg.data, msg2 + strlen(msg2), 10);
+ responser2.publish(&str_msg2);
}
-
}
// button_msg: Int8
@@ -121,19 +148,13 @@
switch (button_msg.data)
{
case 1:
- eject_1 = 1 - eject_1;
- eject_2 = 1 - eject_2;
- strcpy(msg + len, "x is pressed, eject_1 is ");
- msg[strlen(msg)] = '0' + int(eject_1);
- // ---------- added for clearing n_loaded -------------
- n_loaded = 6;
- strcpy(msg + strlen(msg), "n_loaded is ");
- msg[strlen(msg)] = '0' + n_loaded;
- // ---------- end --------------
+ valve = 1 - valve;
+ strcpy(msg + len, "x is pressed, valve is ");
+ msg[strlen(msg)] = '0' + int(valve);
break;
case -1:
- strcpy(msg + len, "x is released, eject_2 is ");
- msg[strlen(msg)] = '0' + int(eject_2);
+// strcpy(msg + len, "x is released, eject_2 is ");
+// msg[strlen(msg)] = '0' + int(eject_2);
break;
case 2:
lock = 1 - lock;
@@ -146,6 +167,7 @@
break;
case 3:
pin3 = 1;
+ auto_shooting_goToTargetPos = true;
strcpy(msg + len, "triangle is pressed, pin3 is ");
msg[strlen(msg)] = '0' + int(pin3);
break;
@@ -176,27 +198,29 @@
msg[strlen(msg)] = '0' + int(pin2);
break;
case 6:
- load_manual = 0;
- strcpy(msg + len, "right arrow is pressed. ");
+ strcpy(msg + len, "right arrow is pressed. ");
msg[strlen(msg)] = '0' + n_loaded;
dc_motor1.set_out(0,1);
n_loaded++;
- while(firing2 == 0);
+ while(firing2 == 0)
+ {
+ wait(0.05);
+ }
if(n_loaded <= 5)
{
- dc_motor1.set_out(0,0.5);
- wait(1.7);
+ dc_motor1.set_out(0,0.7);
+ wait(1.4);
dc_motor1.set_out(0,0);
}
else
{
- load_manual = 1;
- strcpy(msg + len, "right arrow is pressed. ");
+ wait(0.21);
+ dc_motor1.set_out(0,0);
strcpy(msg + strlen(msg), "n_loaded cleared!");
- n_loaded = 0;
+ n_loaded = 0;
+
}
-
break;
case -6:
strcpy(msg + len, "right arrow is released. ");
@@ -215,41 +239,91 @@
case 8:
strcpy(msg + len, "left arrow is pressed, phase is ");
msg[strlen(msg)] = '0' + stage_pick;
+ strcpy(msg + strlen(msg), " ");
+
if(stage_pick == 0)
{
- dc_motor2.set_out(0, 1);
-// if(firing3 == 1)
-// {
-// dc_motor2.set_out(0,0);
-// strcpy(msg + strlen(msg), ", firing pin detected. ");
-// }
+ strcpy(msg + strlen(msg), "dc_motor reset ");
+ dc_motor2.set_out(0.5, 0);
+ while(firing3 == 0);
+ if(firing3 == 1)
+ {
+ dc_motor2.set_out(0,0);
+ strcpy(msg + strlen(msg), "short: firing3 pin detected ");
+ }
stage_pick++;
- eject_1 = 1; eject_2 = 0;
- strcpy(msg + strlen(msg), "rotate out");
+// eject_1 = 1; eject_2 = 0;
}
else if(stage_pick == 1)
{
- dc_motor2.set_out(0.5,0);
- wait(0.3);
- dc_motor2.set_out(0,0);
- stage_pick++;
- strcpy(msg + strlen(msg), "rotate phase 1");
+ strcpy(msg + strlen(msg), "faulhaber 2 RefPos ");
+ faul2_1 = 1;
+ while(firing4 == 0);
+ faul2_1 = 0;
+ strcpy(msg + strlen(msg), "long: firing4 detected "); //內
+ stage_pick++;
+
}
else if(stage_pick == 2)
{
- dc_motor2.set_out(0.5,0);
- wait(0.3);
+ strcpy(msg + strlen(msg), "dc_motor rotate to pick up ");
+ dc_motor2.set_out(0,0.5);
+ wait(0.2);
dc_motor2.set_out(0,0);
+ stage_pick++;
+// stage_pick = 0;
+
+ }
+ else if(stage_pick == 3)
+ {
+ strcpy(msg + strlen(msg), "faul2 to pick the arrows "); // 外
+ faul2_2 = 1;
+ while(firing5 == 0);
+ wait(0.3);
+ faul2_2 = 0;
+ strcpy(msg + strlen(msg), "Firing5 touched ");
+
+ stage_pick++; // use firing 5
+// stage_pick = 0;
+ }
+ else if(stage_pick == 4)
+ {
+ strcpy(msg + strlen(msg), "faul2 rotate to vertical \n\r"); // 內一啲
+ faul2_1 = 1;
+ wait(2.6);
+ faul2_1 = 0;
+ stage_pick++;
+// stage_pick = 0;
+ }
+ else if(stage_pick == 5)
+ {
+ strcpy(msg + strlen(msg), "dc_motor rotate to final pos\n\r");
+ dc_motor2.set_out(0,1);
+// wait(2); // when there is load; otherwise 0.3
+ while(firing6 == 0);
+// {
+// wait(0.025);
+// }
+ dc_motor2.set_out(0,0);
+ stage_pick++;
+// stage_pick = 0;
+ }
+ else if(stage_pick == 6)
+ {
+ strcpy(msg + strlen(msg), "faul2 rotate to final pos \n\r"); // 內一啲
+ faul2_1 = 1;
+ wait(1.9);
+ faul2_1 = 0;
stage_pick = 0;
- strcpy(msg + strlen(msg), "rotate phase 2");
}
+
break;
case -8:
-// strcpy(msg + len, "left arrow is released, left is ");
+
break;
case 9:
strcpy(msg + len, "L1 is pressed");
-// msg[strlen(msg)] = '0' + auto_shooting;
+
break;
case -9:
// pin3 = 0; pin4 = 0;
@@ -261,6 +335,7 @@
dc_motor2.set_out(0,0);
auto_shooting = 1;
pin4 = 1;
+ n_loaded = 6;
strcpy(msg + len, "R1 is pressed, auto_shooting is ");
msg[strlen(msg)] = '0' + auto_shooting;
break;
@@ -271,7 +346,24 @@
strcpy(msg + len, "R1 is released");
break;
case 11:
- strcpy(msg + len, "share is pressed");
+ strcpy(msg + len, "share is pressed");
+ strcpy(msg + strlen(msg), "Start resetting Faulhaber: firing1 is ");
+ msg[strlen(msg)] = '0' + firing1.read();
+ responser.publish(&str_msg);
+ if(!faul_reset && firing1 == 0)
+ {
+ pin2 = 0; pin1 = 1; pin3 = 1; // 101
+ memset(msg, 0, sizeof(msg));
+
+// faul_reset = 1;
+ }
+ while(firing1 == 0);
+ pin1 = 0; pin2 = 1; pin3 = 1; // 110
+ wait(0.3);
+ pin1 = 0; pin2 = 0; pin3 = 0;
+// memset(msg, 0, sizeof(msg));
+ strcpy(msg + strlen(msg), " case 11 firing1 is touched");
+
break;
case -11:
strcpy(msg + len, "share is released");
@@ -323,13 +415,15 @@
// Initiate IOs, keep everything at rest
myled = 0;
pin1 = 0;pin2 = 0; pin3 = 0; lock = 0;
- eject_1 = 0; eject_2 = 1;
+ valve = 0; faul2_1 = 0; faul2_2 = 0;
// Initiate firing pins
firing1.mode(PullUp); // default 0
firing2.mode(PullUp); // default 0
firing3.mode(PullUp); // default 0
-
+ firing4.mode(PullUp); // default 0
+ firing5.mode(PullUp); // default 0
+ firing6.mode(PullUp); // default 0
// Reset dc motor
dc_motor1.reset();
// dc_motor1.set_pid(0.008, 0, 0, 0.0);
@@ -339,9 +433,7 @@
// aout.period_us(0.01);
aout = 0;
// Main programming
- int start = 0; // control variable for resetting shooter
- int tmp = 0;
-
+
while(true) {
nh.spinOnce();
@@ -349,55 +441,8 @@
myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
else
myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
- if(button == 1)
- {
- myled = 0;
- if(!faul_reset && firing1 == 0)
- {
- pin2 = 0; pin1 = 1; pin3 = 1; // 101
- start = 1;
- memset(msg, 0, sizeof(msg));
- strcpy(msg, "Start resetting Faulhaber: firing1 is ");
- msg[strlen(msg)] = '0' + firing1.read();
- responser.publish(&str_msg);
-// faul_reset = 1;
- }
- else
- {
- dc_motor1.set_out(0,1);
- load_manual = 1;
- }
- while(button == 1);
- wait(0.2);
- memset(msg, 0, sizeof(msg));
- strcpy(msg, "user button is pressed, faul_reset is ");
- msg[strlen(msg)] = '0' + faul_reset;
- strcpy(msg + strlen(msg), ", firing2 is ");
- msg[strlen(msg)] = '0' + firing2.read();
- strcpy(msg + strlen(msg), ", load_manual is ");
- msg[strlen(msg)] = '0' + load_manual;
- responser.publish(&str_msg);
- }
- if(load_manual && firing2 == 1)
- {
- wait(0.17);
- dc_motor1.set_out(0,0);
- memset(msg, 0, sizeof(msg));
- strcpy(msg, "firing2 is touched: ");
- msg[strlen(msg)] = '0' + firing2.read();
- }
- if(firing1.read() == 1 && start == 1)
- {
- pin1 = 0; pin2 = 1; pin3 = 1; // 110
- wait(0.3);
- memset(msg, 0, sizeof(msg));
- strcpy(msg, "firing1 is touched: ");
- msg[strlen(msg)] = '0' + firing1.read();
- responser.publish(&str_msg);
- pin3 = 0; pin2 = 0; pin1 = 0;
- start = 0;
-
- }
+
+
}
}
@@ -502,7 +547,7 @@
// -----------R2------------
if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
{
- strcpy(msg + len, "L3: the value is ");
+ strcpy(msg + len, "R2: the value is ");
len = strlen(msg);
itoa(joystick_msg.data[2], msg + len, 10);