MBed_TR1 - MBed program for TR1 functions

Robocon 2021 » Code » MBed_TR1

Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@6:0b79b90b83a2, 2021-06-16 (annotated)

Committer:: ftppr
Date:: Wed Jun 16 10:10:57 2021 +0000
Revision:: 6:0b79b90b83a2
Parent:: 5:03af248f3f7c
Child:: 7:66c6e14a1d97
Child:: 8:9abe58bd7c07

Who changed what in which revision?

User	Revision	Line number	New contents of line
ftppr	0:1bf7e0755f40	1	// ROS Connect Baud Rate
ftppr	0:1bf7e0755f40	2	#define BAUD_RATE 115200
ftppr	0:1bf7e0755f40	3
ftppr	0:1bf7e0755f40	4	#include "mbed.h"
ftppr	0:1bf7e0755f40	5	#include "Servo.h"
ftppr	0:1bf7e0755f40	6	#include <ros.h>
ftppr	0:1bf7e0755f40	7	#include <std_msgs/Int8.h>
ftppr	5:03af248f3f7c	8	#include <std_msgs/Int16.h>
ftppr	0:1bf7e0755f40	9	#include <std_msgs/String.h>
ftppr	0:1bf7e0755f40	10	#include <std_msgs/Int16MultiArray.h>
ftppr	2:e570143bf35e	11	#include "DC_Motor_Controller.h"
ftppr	5:03af248f3f7c	12	//#include "DC_Motor.h"
ftppr	2:e570143bf35e	13
ftppr	2:e570143bf35e	14	// Declare Functions
ftppr	0:1bf7e0755f40	15	void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
ftppr	0:1bf7e0755f40	16	void msgCb_button(const std_msgs::Int8 &button_msg);
ftppr	5:03af248f3f7c	17	void msgCam(const std_msgs::Int16 &cam_msg);
ftppr	5:03af248f3f7c	18
ftppr	2:e570143bf35e	19	void itoa(int num, char *str, int radix);
ftppr	0:1bf7e0755f40	20
ftppr	2:e570143bf35e	21	// Define ros variables
ftppr	2:e570143bf35e	22	ros::NodeHandle nh;
ftppr	2:e570143bf35e	23	// Subs
ftppr	0:1bf7e0755f40	24	ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
ftppr	0:1bf7e0755f40	25	ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
ftppr	6:0b79b90b83a2	26	//ros::Subscriber<std_msgs::Int16> sub_cam("PotCoordinate", msgCam);
ftppr	5:03af248f3f7c	27	ros::Subscriber<std_msgs::Int16> sub_cam("PotDistance", msgCam);
ftppr	2:e570143bf35e	28	// Pubs
ftppr	0:1bf7e0755f40	29	std_msgs::String str_msg;
ftppr	5:03af248f3f7c	30	std_msgs::String str_msg2;
ftppr	0:1bf7e0755f40	31	ros::Publisher responser("display", &str_msg);
ftppr	5:03af248f3f7c	32	ros::Publisher responser2("show_cam", &str_msg2);
ftppr	0:1bf7e0755f40	33
ftppr	2:e570143bf35e	34	// ============ Define IOs =============
ftppr	0:1bf7e0755f40	35
ftppr	2:e570143bf35e	36	// External Button
ftppr	0:1bf7e0755f40	37	DigitalIn button(USER_BUTTON);
ftppr	2:e570143bf35e	38	// LED
ftppr	2:e570143bf35e	39	DigitalOut myled(LED1);
ftppr	2:e570143bf35e	40	// Faulhaber pins
ftppr	5:03af248f3f7c	41	DigitalOut pin1 = A3; // pin 1 of faulhaber
ftppr	5:03af248f3f7c	42	DigitalOut pin2 = A5; // pin 2 of faulhaber
ftppr	5:03af248f3f7c	43	DigitalOut pin3 = A4; // pin 3 of faulhaber
ftppr	5:03af248f3f7c	44	DigitalOut pin4 = A0; // pin 4 of faulhaber
ftppr	5:03af248f3f7c	45	AnalogOut aout = D13;
ftppr	2:e570143bf35e	46	// Electromagnet
ftppr	2:e570143bf35e	47	DigitalOut lock = D14;
ftppr	2:e570143bf35e	48	// Firing pin
ftppr	5:03af248f3f7c	49	DigitalIn firing1 = D9; // Reset Faulhaber
ftppr	2:e570143bf35e	50	DigitalIn firing2 = D15; // Load 1 arrow
ftppr	2:e570143bf35e	51	DigitalIn firing3 = D2; // Pick, small
ftppr	2:e570143bf35e	52	// Valves
ftppr	5:03af248f3f7c	53	DigitalOut eject_1 = D4; // valve 1
ftppr	5:03af248f3f7c	54	DigitalOut eject_2 = D7; // valve 2
ftppr	2:e570143bf35e	55	// DC motors
ftppr	5:03af248f3f7c	56	DC_Motor_PID dc_motor1(D6, D5, PE_6, PF_8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
ftppr	5:03af248f3f7c	57	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
ftppr	2:e570143bf35e	58
ftppr	2:e570143bf35e	59	// Global Control variable
ftppr	2:e570143bf35e	60	char msg[200] = {0}; // msg for responser
ftppr	5:03af248f3f7c	61	char msg2[200] = {0}; // msg for cam show
ftppr	2:e570143bf35e	62	int stage_pick = 0; // stage of picking up arrows from the rack
ftppr	2:e570143bf35e	63	int n_loaded = 0; // number of arrows.
ftppr	2:e570143bf35e	64	int faul_reset = 0;
ftppr	2:e570143bf35e	65	int load_manual = 1; // tune the loading 1 arrow manually
ftppr	5:03af248f3f7c	66	int auto_shooting = 0;
ftppr	5:03af248f3f7c	67	int smooth = 0;
ftppr	5:03af248f3f7c	68
ftppr	5:03af248f3f7c	69	// cam callback
ftppr	5:03af248f3f7c	70	void msgCam(const std_msgs::Int16 &cam_msg)
ftppr	5:03af248f3f7c	71	{
ftppr	6:0b79b90b83a2	72	float minimum = 250;
ftppr	6:0b79b90b83a2	73	float maximum = 600;
ftppr	6:0b79b90b83a2	74	/*
ftppr	6:0b79b90b83a2	75	if( cam_msg.data > 0)
ftppr	6:0b79b90b83a2	76	{
ftppr	6:0b79b90b83a2	77	aout = (maximum - float(cam_msg.data)) / (maximum - minimum);
ftppr	6:0b79b90b83a2	78	if(auto_shooting)
ftppr	6:0b79b90b83a2	79	{
ftppr	6:0b79b90b83a2	80	memset(msg2, 0, sizeof(msg2));
ftppr	6:0b79b90b83a2	81	strcpy(msg2, "Cam distance: ");
ftppr	6:0b79b90b83a2	82	itoa(cam_msg.data, msg2 + strlen(msg2), 10);
ftppr	6:0b79b90b83a2	83	strcpy(msg2 + strlen(msg2), ", aout(1000): ");
ftppr	6:0b79b90b83a2	84	itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
ftppr	6:0b79b90b83a2	85	// sprintf("Cam distance: %d, %.4f", data, aout.read());
ftppr	6:0b79b90b83a2	86	responser2.publish(&str_msg2);
ftppr	6:0b79b90b83a2	87	}
ftppr	6:0b79b90b83a2	88	}*/
ftppr	6:0b79b90b83a2	89	if( cam_msg.data > 0 && auto_shooting)
ftppr	5:03af248f3f7c	90	{
ftppr	6:0b79b90b83a2	91	int outValue[3] = [cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10];
ftppr	6:0b79b90b83a2	92	if(auto_shooting)
ftppr	6:0b79b90b83a2	93	{
ftppr	6:0b79b90b83a2	94	memset(msg2, 0, sizeof(msg2));
ftppr	6:0b79b90b83a2	95	strcpy(msg2, "Cam distance: ");
ftppr	6:0b79b90b83a2	96	itoa(cam_msg.data, msg2 + strlen(msg2), 10);
ftppr	6:0b79b90b83a2	97	strcpy(msg2 + strlen(msg2), ", aout(1000): ");
ftppr	6:0b79b90b83a2	98	itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
ftppr	6:0b79b90b83a2	99	// sprintf("Cam distance: %d, %.4f", data, aout.read());
ftppr	6:0b79b90b83a2	100	responser2.publish(&str_msg2);
ftppr	6:0b79b90b83a2	101	for(int i = 0; i<3; i++){
ftppr	6:0b79b90b83a2	102	outValue[i]/8;
ftppr	6:0b79b90b83a2	103	(outValue[i]/4)%2;
ftppr	6:0b79b90b83a2	104	(outValue[i]/2
ftppr	6:0b79b90b83a2	105	(outValue[i]%2;
ftppr	6:0b79b90b83a2	106	}
ftppr	6:0b79b90b83a2	107	}
ftppr	5:03af248f3f7c	108	}
ftppr	6:0b79b90b83a2	109
ftppr	5:03af248f3f7c	110	}
ftppr	2:e570143bf35e	111
ftppr	2:e570143bf35e	112	// button_msg: Int8
ftppr	2:e570143bf35e	113	void msgCb_button(const std_msgs::Int8 &button_msg)
ftppr	2:e570143bf35e	114	{
ftppr	2:e570143bf35e	115	memset(msg, 0, sizeof(msg));
ftppr	2:e570143bf35e	116	strcpy(msg, "Received ");
ftppr	2:e570143bf35e	117	itoa(button_msg.data, msg + 9, 10);
ftppr	2:e570143bf35e	118	int len = strlen(msg);
ftppr	2:e570143bf35e	119	msg[len++] = ';';
ftppr	2:e570143bf35e	120	msg[len++] = ' ';
ftppr	2:e570143bf35e	121	switch (button_msg.data)
ftppr	2:e570143bf35e	122	{
ftppr	2:e570143bf35e	123	case 1:
ftppr	2:e570143bf35e	124	eject_1 = 1 - eject_1;
ftppr	2:e570143bf35e	125	eject_2 = 1 - eject_2;
ftppr	2:e570143bf35e	126	strcpy(msg + len, "x is pressed, eject_1 is ");
ftppr	2:e570143bf35e	127	msg[strlen(msg)] = '0' + int(eject_1);
ftppr	5:03af248f3f7c	128	// ---------- added for clearing n_loaded -------------
ftppr	5:03af248f3f7c	129	n_loaded = 6;
ftppr	5:03af248f3f7c	130	strcpy(msg + strlen(msg), "n_loaded is ");
ftppr	5:03af248f3f7c	131	msg[strlen(msg)] = '0' + n_loaded;
ftppr	5:03af248f3f7c	132	// ---------- end --------------
ftppr	2:e570143bf35e	133	break;
ftppr	2:e570143bf35e	134	case -1:
ftppr	2:e570143bf35e	135	strcpy(msg + len, "x is released, eject_2 is ");
ftppr	2:e570143bf35e	136	msg[strlen(msg)] = '0' + int(eject_2);
ftppr	2:e570143bf35e	137	break;
ftppr	2:e570143bf35e	138	case 2:
ftppr	2:e570143bf35e	139	lock = 1 - lock;
ftppr	2:e570143bf35e	140	strcpy(msg + len, "o is pressed, lock is: ");
ftppr	2:e570143bf35e	141	msg[strlen(msg)] = '0' + int(lock);
ftppr	2:e570143bf35e	142
ftppr	2:e570143bf35e	143	break;
ftppr	2:e570143bf35e	144	case -2:
ftppr	2:e570143bf35e	145	strcpy(msg + len, "o is released");
ftppr	2:e570143bf35e	146	break;
ftppr	2:e570143bf35e	147	case 3:
ftppr	5:03af248f3f7c	148	pin3 = 1;
ftppr	5:03af248f3f7c	149	strcpy(msg + len, "triangle is pressed, pin3 is ");
ftppr	5:03af248f3f7c	150	msg[strlen(msg)] = '0' + int(pin3);
ftppr	2:e570143bf35e	151	break;
ftppr	2:e570143bf35e	152	case -3:
ftppr	5:03af248f3f7c	153	pin3 = 0;
ftppr	5:03af248f3f7c	154	strcpy(msg + len, "triangle is released, pin3 is ");
ftppr	5:03af248f3f7c	155	msg[strlen(msg)] = '0' + int(pin3);
ftppr	2:e570143bf35e	156	break;
ftppr	2:e570143bf35e	157	case 4:
ftppr	2:e570143bf35e	158	strcpy(msg + len, "square is pressed");
ftppr	5:03af248f3f7c	159	pin1 = 1; pin2 = 1; pin3 = 0; lock = 1;
ftppr	2:e570143bf35e	160	break;
ftppr	2:e570143bf35e	161	case -4:
ftppr	2:e570143bf35e	162	strcpy(msg + len, "square is released");
ftppr	5:03af248f3f7c	163	pin1 = 0; pin2 = 0;
ftppr	2:e570143bf35e	164	break;
ftppr	2:e570143bf35e	165	case 5:
ftppr	5:03af248f3f7c	166
ftppr	5:03af248f3f7c	167	pin1 = 0;
ftppr	5:03af248f3f7c	168	pin3 = 0;
ftppr	5:03af248f3f7c	169	pin2 = 1;
ftppr	5:03af248f3f7c	170	strcpy(msg + len, "down arrow is pressed, pin2 is ");
ftppr	5:03af248f3f7c	171	msg[strlen(msg)] = '0' + int(pin2);
ftppr	2:e570143bf35e	172	break;
ftppr	2:e570143bf35e	173	case -5:
ftppr	5:03af248f3f7c	174	pin2 = 0; pin4 = 0;
ftppr	5:03af248f3f7c	175	strcpy(msg + len, "down arrow is released, pin2 is ");
ftppr	5:03af248f3f7c	176	msg[strlen(msg)] = '0' + int(pin2);
ftppr	2:e570143bf35e	177	break;
ftppr	2:e570143bf35e	178	case 6:
ftppr	2:e570143bf35e	179	load_manual = 0;
ftppr	2:e570143bf35e	180	strcpy(msg + len, "right arrow is pressed. ");
ftppr	2:e570143bf35e	181	msg[strlen(msg)] = '0' + n_loaded;
ftppr	2:e570143bf35e	182	dc_motor1.set_out(0,1);
ftppr	2:e570143bf35e	183	n_loaded++;
ftppr	3:2441bcef5885	184	while(firing2 == 0);
ftppr	4:d5502eb8adcd	185
ftppr	4:d5502eb8adcd	186	if(n_loaded <= 5)
ftppr	4:d5502eb8adcd	187	{
ftppr	5:03af248f3f7c	188	dc_motor1.set_out(0,0.5);
ftppr	5:03af248f3f7c	189	wait(1.7);
ftppr	4:d5502eb8adcd	190	dc_motor1.set_out(0,0);
ftppr	4:d5502eb8adcd	191	}
ftppr	4:d5502eb8adcd	192	else
ftppr	4:d5502eb8adcd	193	{
ftppr	4:d5502eb8adcd	194	load_manual = 1;
ftppr	4:d5502eb8adcd	195	strcpy(msg + len, "right arrow is pressed. ");
ftppr	4:d5502eb8adcd	196	strcpy(msg + strlen(msg), "n_loaded cleared!");
ftppr	5:03af248f3f7c	197	n_loaded = 0;
ftppr	4:d5502eb8adcd	198	}
ftppr	4:d5502eb8adcd	199
ftppr	2:e570143bf35e	200	break;
ftppr	6:0b79b90b83a2	201	case -6:
ftppr	2:e570143bf35e	202	strcpy(msg + len, "right arrow is released. ");
ftppr	2:e570143bf35e	203	break;
ftppr	6:0b79b90b83a2	204	case 7:
ftppr	5:03af248f3f7c	205	pin2 = 0;
ftppr	5:03af248f3f7c	206	pin1 = 1;
ftppr	5:03af248f3f7c	207	strcpy(msg + len, "up arrow is pressed, pin1 is");
ftppr	5:03af248f3f7c	208	msg[strlen(msg)] = '0' + int(pin1);
ftppr	2:e570143bf35e	209	break;
ftppr	2:e570143bf35e	210	case -7:
ftppr	6:0b79b90b83a2	211	pin1 = 0;
ftppr	5:03af248f3f7c	212	strcpy(msg + len, "up arrow is released, pin1 is");
ftppr	5:03af248f3f7c	213	msg[strlen(msg)] = '0' + int(pin1);
ftppr	2:e570143bf35e	214	break;
ftppr	2:e570143bf35e	215	case 8:
ftppr	6:0b79b90b83a2	216	strcpy(msg + len, "left arrow is pressed, phase is ");
ftppr	2:e570143bf35e	217	msg[strlen(msg)] = '0' + stage_pick;
ftppr	2:e570143bf35e	218	if(stage_pick == 0)
ftppr	2:e570143bf35e	219	{
ftppr	6:0b79b90b83a2	220	dc_motor2.set_out(0, 1);
ftppr	6:0b79b90b83a2	221	// if(firing3 == 1)
ftppr	6:0b79b90b83a2	222	// {
ftppr	6:0b79b90b83a2	223	// dc_motor2.set_out(0,0);
ftppr	6:0b79b90b83a2	224	// strcpy(msg + strlen(msg), ", firing pin detected. ");
ftppr	6:0b79b90b83a2	225	// }
ftppr	2:e570143bf35e	226	stage_pick++;
ftppr	2:e570143bf35e	227	eject_1 = 1; eject_2 = 0;
ftppr	6:0b79b90b83a2	228	strcpy(msg + strlen(msg), "rotate out");
ftppr	2:e570143bf35e	229	}
ftppr	2:e570143bf35e	230	else if(stage_pick == 1)
ftppr	2:e570143bf35e	231	{
ftppr	2:e570143bf35e	232	dc_motor2.set_out(0.5,0);
ftppr	2:e570143bf35e	233	wait(0.3);
ftppr	2:e570143bf35e	234	dc_motor2.set_out(0,0);
ftppr	2:e570143bf35e	235	stage_pick++;
ftppr	6:0b79b90b83a2	236	strcpy(msg + strlen(msg), "rotate phase 1");
ftppr	2:e570143bf35e	237	}
ftppr	2:e570143bf35e	238	else if(stage_pick == 2)
ftppr	2:e570143bf35e	239	{
ftppr	2:e570143bf35e	240	dc_motor2.set_out(0.5,0);
ftppr	2:e570143bf35e	241	wait(0.3);
ftppr	2:e570143bf35e	242	dc_motor2.set_out(0,0);
ftppr	2:e570143bf35e	243	stage_pick = 0;
ftppr	6:0b79b90b83a2	244	strcpy(msg + strlen(msg), "rotate phase 2");
ftppr	2:e570143bf35e	245	}
ftppr	2:e570143bf35e	246	break;
ftppr	2:e570143bf35e	247	case -8:
ftppr	6:0b79b90b83a2	248	// strcpy(msg + len, "left arrow is released, left is ");
ftppr	2:e570143bf35e	249	break;
ftppr	2:e570143bf35e	250	case 9:
ftppr	6:0b79b90b83a2	251	strcpy(msg + len, "L1 is pressed");
ftppr	6:0b79b90b83a2	252	// msg[strlen(msg)] = '0' + auto_shooting;
ftppr	2:e570143bf35e	253	break;
ftppr	2:e570143bf35e	254	case -9:
ftppr	6:0b79b90b83a2	255	// pin3 = 0; pin4 = 0;
ftppr	2:e570143bf35e	256	strcpy(msg + len, "L1 is released");
ftppr	2:e570143bf35e	257	break;
ftppr	2:e570143bf35e	258	case 10:
ftppr	6:0b79b90b83a2	259
ftppr	2:e570143bf35e	260	dc_motor1.set_out(0,0);
ftppr	2:e570143bf35e	261	dc_motor2.set_out(0,0);
ftppr	6:0b79b90b83a2	262	auto_shooting = 1;
ftppr	6:0b79b90b83a2	263	pin4 = 1;
ftppr	6:0b79b90b83a2	264	strcpy(msg + len, "R1 is pressed, auto_shooting is ");
ftppr	6:0b79b90b83a2	265	msg[strlen(msg)] = '0' + auto_shooting;
ftppr	2:e570143bf35e	266	break;
ftppr	2:e570143bf35e	267	case -10:
ftppr	6:0b79b90b83a2	268	wait(0.1);
ftppr	6:0b79b90b83a2	269	auto_shooting = 0;
ftppr	6:0b79b90b83a2	270	pin4 = 0;
ftppr	2:e570143bf35e	271	strcpy(msg + len, "R1 is released");
ftppr	2:e570143bf35e	272	break;
ftppr	2:e570143bf35e	273	case 11:
ftppr	6:0b79b90b83a2	274	strcpy(msg + len, "share is pressed");
ftppr	2:e570143bf35e	275	break;
ftppr	2:e570143bf35e	276	case -11:
ftppr	2:e570143bf35e	277	strcpy(msg + len, "share is released");
ftppr	2:e570143bf35e	278	break;
ftppr	2:e570143bf35e	279	case 12:
ftppr	2:e570143bf35e	280	strcpy(msg + len, "option is pressed");
ftppr	2:e570143bf35e	281	break;
ftppr	2:e570143bf35e	282	case -12:
ftppr	2:e570143bf35e	283	strcpy(msg + len, "option is released");
ftppr	2:e570143bf35e	284	break;
ftppr	2:e570143bf35e	285	case 13:
ftppr	5:03af248f3f7c	286	pin1 = 1; pin2 = 1; pin3 = 1;
ftppr	2:e570143bf35e	287	strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
ftppr	2:e570143bf35e	288	break;
ftppr	2:e570143bf35e	289	case -13:
ftppr	5:03af248f3f7c	290	pin1 = 0; pin2 = 0; pin3= 0;
ftppr	2:e570143bf35e	291	strcpy(msg + len, "playstation button is released");
ftppr	2:e570143bf35e	292	break;
ftppr	2:e570143bf35e	293	default:
ftppr	2:e570143bf35e	294	len = strlen(msg);
ftppr	2:e570143bf35e	295	strcpy(msg + len, "Unrecognized pattern");
ftppr	2:e570143bf35e	296	break;
ftppr	2:e570143bf35e	297	}
ftppr	2:e570143bf35e	298
ftppr	2:e570143bf35e	299	responser.publish(&str_msg);
ftppr	2:e570143bf35e	300	}
ftppr	2:e570143bf35e	301
ftppr	2:e570143bf35e	302
ftppr	2:e570143bf35e	303	// --------------- main -----------------
ftppr	2:e570143bf35e	304	int main()
ftppr	2:e570143bf35e	305	{
ftppr	2:e570143bf35e	306
ftppr	2:e570143bf35e	307	// -> Set Baud Rate
ftppr	2:e570143bf35e	308	nh.getHardware()->setBaud(BAUD_RATE);
ftppr	2:e570143bf35e	309	nh.initNode();
ftppr	2:e570143bf35e	310	// Advertise publishers
ftppr	2:e570143bf35e	311	nh.advertise(responser);
ftppr	5:03af248f3f7c	312	nh.advertise(responser2);
ftppr	2:e570143bf35e	313	// Initiate display
ftppr	5:03af248f3f7c	314	// msg = "------- LAST DAY -------";
ftppr	2:e570143bf35e	315	str_msg.data = msg;
ftppr	5:03af248f3f7c	316	str_msg2.data = msg2;
ftppr	2:e570143bf35e	317	// responser.publish(&str_msg);
ftppr	5:03af248f3f7c	318	// responser2.publish(&str_msg2);
ftppr	2:e570143bf35e	319	// Initiate subscribers
ftppr	2:e570143bf35e	320	nh.subscribe(sub_button);
ftppr	2:e570143bf35e	321	nh.subscribe(sub_joystick);
ftppr	5:03af248f3f7c	322	nh.subscribe(sub_cam);
ftppr	2:e570143bf35e	323	// Initiate IOs, keep everything at rest
ftppr	2:e570143bf35e	324	myled = 0;
ftppr	5:03af248f3f7c	325	pin1 = 0;pin2 = 0; pin3 = 0; lock = 0;
ftppr	2:e570143bf35e	326	eject_1 = 0; eject_2 = 1;
ftppr	2:e570143bf35e	327
ftppr	2:e570143bf35e	328	// Initiate firing pins
ftppr	5:03af248f3f7c	329	firing1.mode(PullUp); // default 0
ftppr	6:0b79b90b83a2	330	firing2.mode(PullUp); // default 0
ftppr	6:0b79b90b83a2	331	firing3.mode(PullUp); // default 0
ftppr	2:e570143bf35e	332
ftppr	2:e570143bf35e	333	// Reset dc motor
ftppr	2:e570143bf35e	334	dc_motor1.reset();
ftppr	5:03af248f3f7c	335	// dc_motor1.set_pid(0.008, 0, 0, 0.0);
ftppr	2:e570143bf35e	336	dc_motor2.reset();
ftppr	2:e570143bf35e	337	dc_motor2.set_pid(0.008, 0, 0, 0.0);
ftppr	5:03af248f3f7c	338	// up = 1; pin2 = 1; pin2 = 0;
ftppr	6:0b79b90b83a2	339	// aout.period_us(0.01);
ftppr	6:0b79b90b83a2	340	aout = 0;
ftppr	2:e570143bf35e	341	// Main programming
ftppr	2:e570143bf35e	342	int start = 0; // control variable for resetting shooter
ftppr	2:e570143bf35e	343	int tmp = 0;
ftppr	2:e570143bf35e	344
ftppr	2:e570143bf35e	345	while(true) {
ftppr	2:e570143bf35e	346	nh.spinOnce();
ftppr	2:e570143bf35e	347
ftppr	2:e570143bf35e	348	if(nh.connected())
ftppr	2:e570143bf35e	349	myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
ftppr	2:e570143bf35e	350	else
ftppr	2:e570143bf35e	351	myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
ftppr	2:e570143bf35e	352	if(button == 1)
ftppr	2:e570143bf35e	353	{
ftppr	2:e570143bf35e	354	myled = 0;
ftppr	3:2441bcef5885	355	if(!faul_reset && firing1 == 0)
ftppr	2:e570143bf35e	356	{
ftppr	6:0b79b90b83a2	357	pin2 = 0; pin1 = 1; pin3 = 1; // 101
ftppr	2:e570143bf35e	358	start = 1;
ftppr	4:d5502eb8adcd	359	memset(msg, 0, sizeof(msg));
ftppr	4:d5502eb8adcd	360	strcpy(msg, "Start resetting Faulhaber: firing1 is ");
ftppr	4:d5502eb8adcd	361	msg[strlen(msg)] = '0' + firing1.read();
ftppr	4:d5502eb8adcd	362	responser.publish(&str_msg);
ftppr	4:d5502eb8adcd	363	// faul_reset = 1;
ftppr	2:e570143bf35e	364	}
ftppr	2:e570143bf35e	365	else
ftppr	2:e570143bf35e	366	{
ftppr	3:2441bcef5885	367	dc_motor1.set_out(0,1);
ftppr	3:2441bcef5885	368	load_manual = 1;
ftppr	2:e570143bf35e	369	}
ftppr	2:e570143bf35e	370	while(button == 1);
ftppr	2:e570143bf35e	371	wait(0.2);
ftppr	2:e570143bf35e	372	memset(msg, 0, sizeof(msg));
ftppr	2:e570143bf35e	373	strcpy(msg, "user button is pressed, faul_reset is ");
ftppr	3:2441bcef5885	374	msg[strlen(msg)] = '0' + faul_reset;
ftppr	3:2441bcef5885	375	strcpy(msg + strlen(msg), ", firing2 is ");
ftppr	3:2441bcef5885	376	msg[strlen(msg)] = '0' + firing2.read();
ftppr	3:2441bcef5885	377	strcpy(msg + strlen(msg), ", load_manual is ");
ftppr	3:2441bcef5885	378	msg[strlen(msg)] = '0' + load_manual;
ftppr	2:e570143bf35e	379	responser.publish(&str_msg);
ftppr	2:e570143bf35e	380	}
ftppr	2:e570143bf35e	381	if(load_manual && firing2 == 1)
ftppr	2:e570143bf35e	382	{
ftppr	3:2441bcef5885	383	wait(0.17);
ftppr	3:2441bcef5885	384	dc_motor1.set_out(0,0);
ftppr	3:2441bcef5885	385	memset(msg, 0, sizeof(msg));
ftppr	3:2441bcef5885	386	strcpy(msg, "firing2 is touched: ");
ftppr	3:2441bcef5885	387	msg[strlen(msg)] = '0' + firing2.read();
ftppr	2:e570143bf35e	388	}
ftppr	3:2441bcef5885	389	if(firing1.read() == 1 && start == 1)
ftppr	2:e570143bf35e	390	{
ftppr	5:03af248f3f7c	391	pin1 = 0; pin2 = 1; pin3 = 1; // 110
ftppr	2:e570143bf35e	392	wait(0.3);
ftppr	2:e570143bf35e	393	memset(msg, 0, sizeof(msg));
ftppr	2:e570143bf35e	394	strcpy(msg, "firing1 is touched: ");
ftppr	2:e570143bf35e	395	msg[strlen(msg)] = '0' + firing1.read();
ftppr	2:e570143bf35e	396	responser.publish(&str_msg);
ftppr	5:03af248f3f7c	397	pin3 = 0; pin2 = 0; pin1 = 0;
ftppr	3:2441bcef5885	398	start = 0;
ftppr	2:e570143bf35e	399
ftppr	2:e570143bf35e	400	}
ftppr	2:e570143bf35e	401	}
ftppr	2:e570143bf35e	402	}
ftppr	0:1bf7e0755f40	403
ftppr	0:1bf7e0755f40	404
ftppr	0:1bf7e0755f40	405	void itoa(int num, char *str, int radix)
ftppr	0:1bf7e0755f40	406	{ /索引表/
ftppr	0:1bf7e0755f40	407	char index[] = "0123456789ABCDEF";
ftppr	0:1bf7e0755f40	408	unsigned unum; /中间变量/
ftppr	0:1bf7e0755f40	409	int i = 0, j, k;
ftppr	0:1bf7e0755f40	410	/确定unum的值/
ftppr	0:1bf7e0755f40	411	if (radix == 10 && num < 0) /十进制负数/
ftppr	0:1bf7e0755f40	412	{
ftppr	0:1bf7e0755f40	413	unum = (unsigned)-num;
ftppr	0:1bf7e0755f40	414	str[i++] = '-';
ftppr	0:1bf7e0755f40	415	}
ftppr	0:1bf7e0755f40	416	else
ftppr	0:1bf7e0755f40	417	unum = (unsigned)num; /其他情况/
ftppr	0:1bf7e0755f40	418	/转换/
ftppr	0:1bf7e0755f40	419	do
ftppr	0:1bf7e0755f40	420	{
ftppr	0:1bf7e0755f40	421	str[i++] = index[unum % (unsigned)radix];
ftppr	0:1bf7e0755f40	422	unum /= radix;
ftppr	0:1bf7e0755f40	423	} while (unum);
ftppr	0:1bf7e0755f40	424	str[i] = '\0';
ftppr	0:1bf7e0755f40	425	/逆序/
ftppr	0:1bf7e0755f40	426	if (str[0] == '-')
ftppr	0:1bf7e0755f40	427	k = 1; /十进制负数/
ftppr	0:1bf7e0755f40	428	else
ftppr	0:1bf7e0755f40	429	k = 0;
ftppr	0:1bf7e0755f40	430
ftppr	0:1bf7e0755f40	431	for (j = k; j <= (i - 1) / 2; j++)
ftppr	0:1bf7e0755f40	432	{
ftppr	0:1bf7e0755f40	433	char temp;
ftppr	0:1bf7e0755f40	434	temp = str[j];
ftppr	0:1bf7e0755f40	435	str[j] = str[i - 1 + k - j];
ftppr	0:1bf7e0755f40	436	str[i - 1 + k - j] = temp;
ftppr	0:1bf7e0755f40	437	}
ftppr	0:1bf7e0755f40	438	}
ftppr	0:1bf7e0755f40	439
ftppr	0:1bf7e0755f40	440	/*
ftppr	0:1bf7e0755f40	441	* joystick_msg: (Left/Right, x/y, value)
ftppr	0:1bf7e0755f40	442	*/
ftppr	0:1bf7e0755f40	443
ftppr	0:1bf7e0755f40	444	void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg)
ftppr	0:1bf7e0755f40	445	{
ftppr	0:1bf7e0755f40	446	// myled = !myled; // blink the led
ftppr	0:1bf7e0755f40	447	memset(msg, 0, sizeof(msg));
ftppr	0:1bf7e0755f40	448	// eg. Received (0,0,-100)
ftppr	0:1bf7e0755f40	449	strcpy(msg, "Received (");
ftppr	0:1bf7e0755f40	450	int len = strlen(msg);
ftppr	0:1bf7e0755f40	451	msg[len++] = '0' + joystick_msg.data[0];
ftppr	0:1bf7e0755f40	452	msg[len++] = ',';
ftppr	0:1bf7e0755f40	453	itoa(joystick_msg.data[1], msg + len, 10);
ftppr	0:1bf7e0755f40	454	len = strlen(msg);
ftppr	0:1bf7e0755f40	455	msg[len++] = ',';
ftppr	0:1bf7e0755f40	456	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	457	len = strlen(msg);
ftppr	0:1bf7e0755f40	458	msg[len++] = ')';
ftppr	0:1bf7e0755f40	459	msg[len++] = ';';
ftppr	0:1bf7e0755f40	460	msg[len++] = ' ';
ftppr	0:1bf7e0755f40	461
ftppr	0:1bf7e0755f40	462	// Elaboration of the message
ftppr	0:1bf7e0755f40	463	// -----------L3------------
ftppr	0:1bf7e0755f40	464	// x
ftppr	0:1bf7e0755f40	465	if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0)
ftppr	0:1bf7e0755f40	466	{
ftppr	0:1bf7e0755f40	467	strcpy(msg + len, "L3: direction is x(horizontal) and the value is ");
ftppr	0:1bf7e0755f40	468	len = strlen(msg);
ftppr	0:1bf7e0755f40	469	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	470	}
ftppr	0:1bf7e0755f40	471	// y
ftppr	0:1bf7e0755f40	472	if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1)
ftppr	0:1bf7e0755f40	473	{
ftppr	0:1bf7e0755f40	474	strcpy(msg + len, "L3: direction is y(vertical) and the value is ");
ftppr	0:1bf7e0755f40	475	len = strlen(msg);
ftppr	0:1bf7e0755f40	476	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	477	}
ftppr	0:1bf7e0755f40	478
ftppr	0:1bf7e0755f40	479	// -----------R3------------
ftppr	0:1bf7e0755f40	480	// x
ftppr	0:1bf7e0755f40	481	if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0)
ftppr	0:1bf7e0755f40	482	{
ftppr	0:1bf7e0755f40	483	strcpy(msg + len, "R3: direction is x(horizontal) and the value is ");
ftppr	0:1bf7e0755f40	484	len = strlen(msg);
ftppr	0:1bf7e0755f40	485	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	486	}
ftppr	0:1bf7e0755f40	487	// y
ftppr	0:1bf7e0755f40	488	if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1)
ftppr	0:1bf7e0755f40	489	{
ftppr	0:1bf7e0755f40	490	strcpy(msg + len, "R3: direction is y(vertical) and the value is ");
ftppr	0:1bf7e0755f40	491	len = strlen(msg);
ftppr	0:1bf7e0755f40	492	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	493	}
ftppr	0:1bf7e0755f40	494
ftppr	0:1bf7e0755f40	495	// -----------L2------------
ftppr	0:1bf7e0755f40	496	if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1)
ftppr	0:1bf7e0755f40	497	{
ftppr	0:1bf7e0755f40	498	strcpy(msg + len, "L2: the value is ");
ftppr	0:1bf7e0755f40	499	len = strlen(msg);
ftppr	0:1bf7e0755f40	500	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	0:1bf7e0755f40	501	}
ftppr	0:1bf7e0755f40	502	// -----------R2------------
ftppr	0:1bf7e0755f40	503	if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
ftppr	0:1bf7e0755f40	504	{
ftppr	0:1bf7e0755f40	505	strcpy(msg + len, "L3: the value is ");
ftppr	0:1bf7e0755f40	506	len = strlen(msg);
ftppr	0:1bf7e0755f40	507	itoa(joystick_msg.data[2], msg + len, 10);
ftppr	2:e570143bf35e	508
ftppr	0:1bf7e0755f40	509	}
ftppr	0:1bf7e0755f40	510
ftppr	0:1bf7e0755f40	511	responser.publish(&str_msg);
ftppr	0:1bf7e0755f40	512	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Jun 2021
Imports:	4
Forks:	0
Commits:	9
Dependents:	0
Dependencies:	4
Followers:	15

main.cpp@6:0b79b90b83a2, 2021-06-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning