fork download
copy 
  1. #include <stdio.h>
  2. #include <string>
  3. #include <iostream>
  4. #include "zmq.h"
  5. #include "zmq_utils.h"
  6. #include <Windows.h>
  7.  
  8. char starter = 29;
  9. char terminator = 30;
  10.  
  11.  
  12. static const char arm_motorshield = 0;
  13. static const char get_milliamps = 1;
  14. static const char get_fault = 2;
  15. static const char set_brakes = 3;
  16. static const char set_speed = 4;
  17. static const char set_brakes_A = 5;
  18. static const char set_brakes_B = 6;
  19. static const char set_speed_A = 7;
  20. static const char set_speed_B = 8;
  21. static const char echo_data = 9;
  22. static const char set_max_speed = 10;
  23. static const char I_AM_ALIVE = 111;
  24. static const char CONNECTION_OK = 69;
  25. static const char UN_ARM = 33;
  26.  
  27. struct Packet
  28. {
  29. private:
  30. 	char data[7];
  31. public:
  32. 	Packet()
  33. 	{
  34. 		data[0] = starter;
  35. 		data[6] = terminator;
  36. 	}
  37. 	Packet(const char action, int value_one, int value_two)
  38. 	{
  39. 		set(action, value_one, value_two);
  40. 	}
  41. 	Packet(const char action, char byte_a, char byte_b, char byte_c, char byte_d)
  42. 	{
  43. 		set(action, byte_a, byte_b, byte_c, byte_d);
  44. 	}
  45. 	void set(const char action, int value_one, int value_two)
  46. 	{
  47. 		data[0] = starter;
  48. 		data[1] = action;
  49. 		data[2] = ((value_one & 0xFF00) >> 8);
  50. 		data[3] = (value_one & 0xFF);
  51. 		data[4] = ((value_two & 0xFF00) >> 8);
  52. 		data[5] = (value_two & 0xFF);
  53. 		data[6] = terminator;
  54. 	}
  55. 	void set(const char action, char byte_a, char byte_b, char byte_c, char byte_d)
  56. 	{
  57. 		data[0] = starter;
  58. 		data[1] = action;
  59. 		data[2] = byte_a;
  60. 		data[3] = byte_b;
  61. 		data[4] = byte_c;
  62. 		data[5] = byte_d;
  63. 		data[6] = terminator;
  64. 	}
  65. 	void set(char * _data, size_t datasize = 7)
  66. 	{
  67. 		if (datasize > 7)
  68. 			datasize = 7;
  69. 		for (size_t i = 0; i < datasize; ++i)
  70. 		{
  71. 			data[i] = _data[i];
  72. 		}
  73. 	}
  74. 	int GetValueOneLR()
  75. 	{
  76. 		return ((data[2] << 8) & data[3]);
  77. 	}
  78. 	int GetValueOneRL()
  79. 	{
  80. 		return ((data[3] << 8) & data[2]);
  81. 	}
  82. 	int GetValueTwoLR()
  83. 	{
  84. 		return ((data[4] << 8) & data[5]);
  85. 	}
  86. 	int GetValueTwoRL()
  87. 	{
  88. 		return ((data[5] << 8) & data[4]);
  89. 	}
  90. 	const char GetAction()
  91. 	{
  92. 		return data[1];
  93. 	}
  94. 	char GetB1()
  95. 	{
  96. 		return data[2];
  97. 	}
  98. 	char GetB2()
  99. 	{
  100. 		return data[3];
  101. 	}
  102. 	char GetB3()
  103. 	{
  104. 		return data[4];
  105. 	}
  106. 	char GetB4()
  107. 	{
  108. 		return data[5];
  109. 	}
  110.  
  111. 	bool valid()
  112. 	{
  113. 		return (data[0] == starter && data[6] == terminator);
  114. 	}
  115. 	char * get()
  116. 	{
  117. 		return &data[0];
  118. 	}
  119. 	size_t size()
  120. 	{
  121. 		return 7;
  122. 	}
  123. };
  124.  
  125. #pragma comment(lib, "../libzmq.lib")
  126.  
  127. void DoCheckSendWithError(void *client, Packet& packetinfo)
  128. {
  129. 	if (zmq_send(client, packetinfo.get(), packetinfo.size(), ZMQ_DONTWAIT) == -1)
  130. 	{
  131. 		std::cout << "ZMQ !!!packetinfo!!! Send error: ";
  132. 		int err = zmq_errno();
  133. 		switch (err)
  134. 		{
  135. 		case EAGAIN:
  136. 			std::cout << "EAGAIN";
  137. 			break;
  138. 		case ENOTSUP:
  139. 			std::cout << "ENOTSUP";
  140. 			break;
  141. 		case EFSM:
  142. 			std::cout << "EFSM";
  143. 			break;
  144. 		case ETERM:
  145. 			std::cout << "ETERM";
  146. 			break;
  147. 		case ENOTSOCK:
  148. 			std::cout << "ENOTSOCK";
  149. 			break;
  150. 		case EINTR:
  151. 			std::cout << "EINTR";
  152. 			break;
  153. 		case EFAULT:
  154. 			std::cout << "EFAULT";
  155. 			break;
  156. 		}
  157. 		std::cout << "\r\n" << std::flush;
  158. 	}
  159. 	else
  160. 	{
  161. 		std::cout << "Send OK\r\n" << std::flush;
  162. 	}
  163. }
  164.  
  165. int main()
  166. {
  167. 	std::cout << "Exit with Left Ctrl+Q\r\n" << std::flush;
  168. 	void *context = zmq_ctx_new();
  169. 	void *client = zmq_socket(context, ZMQ_DEALER);
  170.  
  171. 	if (zmq_connect(client, "tcp://tank.mshome.net:5000") == 0)
  172. 	{
  173. 		std::cout << "CONNECTION OK\r\n" << std::flush;
  174.  
  175. 		char buffer[1024];
  176. 		Packet packetinfo;
  177. 		Packet alive(I_AM_ALIVE,0,0,0,0);
  178. 		ULONGLONG time_last_alive_sent = 0;
  179. 		ULONGLONG timenow;
  180. 		while ((GetAsyncKeyState(VK_LCONTROL) && GetAsyncKeyState('Q')) == 0)
  181. 		{
  182. 			timenow = GetTickCount64();
  183. 			if ((timenow - time_last_alive_sent) > 200)
  184. 			{
  185. 				time_last_alive_sent = timenow;
  186. 				if (zmq_send(client, alive.get(), alive.size(), ZMQ_DONTWAIT) == -1)
  187. 				{
  188. 					std::cout << "ZMQ !!!ALIVE!!! Send error: ";
  189. 					int err = zmq_errno();
  190. 					switch (err)
  191. 					{
  192. 					case EAGAIN:
  193. 						std::cout << "EAGAIN";
  194. 						break;
  195. 					case ENOTSUP:
  196. 						std::cout << "ENOTSUP";
  197. 						break;
  198. 					case EFSM:
  199. 						std::cout << "EFSM";
  200. 						break;
  201. 					case ETERM:
  202. 						std::cout << "ETERM";
  203. 						break;
  204. 					case ENOTSOCK:
  205. 						std::cout << "ENOTSOCK";
  206. 						break;
  207. 					case EINTR:
  208. 						std::cout << "EINTR";
  209. 						break;
  210. 					case EFAULT:
  211. 						std::cout << "EFAULT";
  212. 						break;
  213. 					}
  214. 					std::cout << "\r\n" << std::flush;
  215. 				}
  216. 			}
  217.  
  218. 			char currentpacket;
  219. 			if (zmq_recv(client, &currentpacket, 1, ZMQ_DONTWAIT) != -1)
  220. 			{
  221. 				if (currentpacket == starter)
  222. 				{
  223. 					Sleep(16);
  224. 					buffer[0] = currentpacket;
  225. 					int received = zmq_recv(client, &buffer[1], 6, ZMQ_DONTWAIT);
  226.  
  227. 					if (received == 6)
  228. 					{
  229. 						packetinfo.set(buffer, 7);
  230. 						switch (packetinfo.GetAction())
  231. 						{
  232. 							case get_milliamps:
  233. 							{
  234. 								std::cout << "Current stats:";
  235.  
  236. 								std::cout << "\r\n\tMotor 1: " << (unsigned int)packetinfo.GetValueOneLR() << " mA";
  237. 								std::cout << "\r\n\tMotor 2: " << (unsigned int)packetinfo.GetValueTwoLR() << " mA";
  238.  
  239. 								std::cout << "\r\n\tMotor 1: " << (unsigned int)packetinfo.GetValueOneRL() << " mA";
  240. 								std::cout << "\r\n\tMotor 2: " << (unsigned int)packetinfo.GetValueTwoRL() << " mA";
  241.  
  242. 								std::cout << "\r\n" << std::flush;
  243. 								break;
  244. 							}
  245. 							case get_fault:
  246. 							{
  247. 								std::cout << "Fault stats:";
  248.  
  249. 								std::cout << "\r\n\tMotor 1: " << (bool)packetinfo.GetB1();
  250. 								std::cout << "\r\n\tMotor 2: " << (bool)packetinfo.GetB2();
  251.  
  252. 								std::cout << "\r\n" << std::flush;
  253. 								break;
  254. 							}
  255. 							case CONNECTION_OK:
  256. 							{
  257. 								std::cout << "Connection is still alive!!\r\n" << std::flush;
  258. 								break;
  259. 							}
  260. 							default:
  261. 							{
  262. 								std::cout << "Unknown packet received\r\n" << std::flush;
  263. 								break;
  264. 							}
  265. 						}
  266. 					}
  267. 					else if (received < 5 && received > 0)
  268. 					{
  269. 						std::cout << "Unknown sized packet received: \r\n";
  270. 						std::cout << received << ": [" << std::hex << (int)buffer[0] << "|"
  271. 							<< std::hex << (int)buffer[1] << "|"
  272. 							<< std::hex << (int)buffer[2] << "|"
  273. 							<< std::hex << (int)buffer[3] << "|"
  274. 							<< std::hex << (int)buffer[4] << "|"
  275. 							<< std::hex << (int)buffer[5] << "|"
  276. 							<< std::hex << (int)buffer[6] << "]" << "\r\n" << std::flush;
  277. 					}
  278. 				}
  279. 			}
  280.  
  281. 			if (GetAsyncKeyState('W') & 1)
  282. 			{
  283. 				packetinfo.set(set_speed, 200, 200);
  284. 				DoCheckSendWithError(client, packetinfo);
  285. 			}
  286. 			if (GetAsyncKeyState('S') & 1)
  287. 			{
  288. 				packetinfo.set(set_brakes, 400, 400);
  289. 				DoCheckSendWithError(client, packetinfo);
  290. 			}
  291. 			if (GetAsyncKeyState('Z') & 1)
  292. 			{
  293. 				packetinfo.set(set_speed, -200, -200);
  294. 				DoCheckSendWithError(client, packetinfo);
  295. 			}
  296. 			if (GetAsyncKeyState('A') & 1)
  297. 			{
  298. 				packetinfo.set(set_speed, -200, 200);
  299. 				DoCheckSendWithError(client, packetinfo);
  300. 			}
  301. 			if (GetAsyncKeyState('D') & 1)
  302. 			{
  303. 				packetinfo.set(set_speed, 200, -200);
  304. 				DoCheckSendWithError(client, packetinfo);
  305. 			}
  306. 			if (GetAsyncKeyState('R') & 1)
  307. 			{
  308. 				packetinfo.set(set_speed, 0, 0);
  309. 				DoCheckSendWithError(client, packetinfo);
  310. 			}
  311. 			if (GetAsyncKeyState('C') & 1)
  312. 			{
  313. 				packetinfo.set(get_milliamps, 0, 0);
  314. 				DoCheckSendWithError(client, packetinfo);
  315. 			}
  316. 			if (GetAsyncKeyState('F') & 1)
  317. 			{
  318. 				packetinfo.set(get_fault, 0, 0);
  319. 				DoCheckSendWithError(client, packetinfo);
  320. 			}
  321. 			if (GetAsyncKeyState('U') & 1)
  322. 			{
  323. 				packetinfo.set(UN_ARM, 5000, 3333);
  324. 				DoCheckSendWithError(client, packetinfo);
  325. 			}
  326. 			Sleep(1);
  327. 		}
  328. 	}
  329. 	else
  330. 	{
  331. 		std::cout << "CONNECTION FAIL\r\n" << std::flush;
  332. 	}
  333. 	zmq_ctx_destroy(&context);
  334. 	return 0;
  335. }
Compilation error #stdin compilation error #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
compilation info 
prog.cpp:4:17: fatal error: zmq.h: No such file or directory
 #include "zmq.h"
                 ^
compilation terminated.
stdout
copy 
Standard output is empty
https://ideone.com/ZeD2CW
language:
C++14 (gcc 8.3)
created:
11 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!