Phoenix5-Linux-Example/example.cpp at master · CrossTheRoadElec/Phoenix5-Linux-Example · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#definePhoenix_No_WPI// remove WPI dependencies
#include"ctre/Phoenix.h"
#include"ctre/phoenix/platform/Platform.hpp"
#include"ctre/phoenix/unmanaged/Unmanaged.h"
#include<chrono>
#include<iostream>
#include<string>
#include<thread>
#include<unistd.h>
#include<SDL2/SDL.h>

usingnamespacectre::phoenix;
usingnamespacectre::phoenix::platform;
usingnamespacectre::phoenix::motorcontrol;
usingnamespacectre::phoenix::motorcontrol::can;

/* make some talons for drive train */
std::string interface = "can0";
TalonSRX talLeft{1, interface}; //Use the specified interface
TalonSRX talRght{0}; //Use the default interface (can0)

voidinitDrive()
{
/* both talons should blink green when driving forward */
	talRght.SetInverted(true);
}

voiddrive(double fwd, double turn)
{
double left = fwd - turn;
double rght = fwd + turn; /* positive turn means turn robot LEFT */

	talLeft.Set(ControlMode::PercentOutput, left);
	talRght.Set(ControlMode::PercentOutput, rght);
}
/** simple wrapper for code cleanup */
voidsleepApp(int ms)
{
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

intmain() {
// Comment out the call if you would rather use the automatically running diag-server, note this requires uninstalling diagnostics from Tuner.
// c_SetPhoenixDiagnosticsStartTime(-1); // disable diag server, instead we will use the diag server stand alone application that Tuner installs

/* setup drive */
initDrive();

while (true) {
/* we are looking for gamepad (first time or after disconnect),
 neutral drive until gamepad (re)connected. */
drive(0, 0);

// wait for gamepad
printf("Waiting for gamepad...\n");
while (true) {
/* SDL seems somewhat fragile, shut it down and bring it up */
SDL_Quit();
SDL_SetHint(SDL_HINT_NO_SIGNAL_HANDLERS, "1"); //so Ctrl-C still works
SDL_Init(SDL_INIT_JOYSTICK);

/* poll for gamepad */
int res = SDL_NumJoysticks();
if (res > 0) { break; }
if (res < 0) { printf("Err = %i\n", res); }

/* yield for a bit */
sleepApp(20);
 }
printf("Waiting for gamepad...Found one\n");

// Open the joystick for reading and store its handle in the joy variable
 SDL_Joystick *joy = SDL_JoystickOpen(0);
if (joy == NULL) {
/* back to top of while loop */
continue;
 }

// Get information about the joystick
constchar *name = SDL_JoystickName(joy);
constint num_axes = SDL_JoystickNumAxes(joy);
constint num_buttons = SDL_JoystickNumButtons(joy);
constint num_hats = SDL_JoystickNumHats(joy);
printf("Now reading from joystick '%s' with:\n"
"%d axes\n"
"%d buttons\n"
"%d hats\n\n",
 name,
 num_axes,
 num_buttons,
 num_hats);

/* I'm using a logitech F350 wireless in D mode.
 If num axis is 6, then gamepad is in X mode, so neutral drive and wait for D mode.
 [SAFETY] This means 'X' becomes our robot-disable button.
 This can be removed if that's not the goal. */
if (num_axes >= 6) {
/* back to top of while loop */
continue;
 }

// Keep reading the state of the joystick in a loop
while (true) {
/* poll for disconnects or bad things */
bool joystick_err = false;
 SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
 joystick_err = true;
break;
 } elseif (event.jdevice.type == SDL_JOYDEVICEREMOVED) {
 joystick_err = true;
break;
 }
 }
if (joystick_err) { break; }

/* grab some stick values */
double y = ((double)SDL_JoystickGetAxis(joy, 1)) / -32767.0;
double turn = ((double)SDL_JoystickGetAxis(joy, 2)) / -32767.0;
drive(y, turn);

/* [SAFETY] only enable drive if top left shoulder button is held down */
if (SDL_JoystickGetButton(joy, 4)) {
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100);
 }

/* loop yield for a bit */
sleepApp(20);
 }

/* we've left the loop, likely due to gamepad disconnect */
drive(0, 0);
SDL_JoystickClose(joy);
printf("gamepad disconnected\n");
	}

SDL_Quit();
return0;
}