-
Notifications
You must be signed in to change notification settings - Fork 0
/
MantisTeleop.c~
executable file
·97 lines (82 loc) · 3.55 KB
/
MantisTeleop.c~
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
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Hubs, S2, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S2_C1_1, motorH, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S2_C1_2, motorI, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S2_C2_1, motorH, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S2_C2_2, motorI, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C2_1, rFinger, tServoStandard)
#pragma config(Servo, srvo_S1_C2_2, lFinger, tServoStandard)
#pragma config(Servo, srvo_S1_C2_3, rWrist, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C2_4, lWrist, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C2_5, rElbow, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C2_6, lElbow, tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
#define JOYSTICK_THRESHOLD 15
#define SHOLDER_MAX_POWER 25
#define SHOLDER_REST_POWER 0
#define ELBOW_MAX_POWER 128
#define ELBOW_REST_POWER 127
#define WRIST_MAX_POWER 28
#define WRIST_REST_POWER 127
void initializeRobot()
{
return;
}
task main()
{
waitForStart(); // wait for start of tele-op phase
initializeRobot();
while (true)
{
getJoystickSettings(joystick);
/*int sholderPowers = 0;
int elbowPowers = ELBOW_REST_POWER;
int wristPowers = WRIST_REST_POWER;
int deltaFingers = 0;
if (abs(joystick.joy1_y1) > JOYSTICK_THRESHOLD)
sholderPowers = joystick.joy1_y1 * SHOLDER_MAX_POWER / 128.0;
if (abs(joystick.joy1_y1) > JOYSTICK_THRESHOLD)
elbowPowers = (joystick.joy1_y1 * WRIST_MAX_POWER / 128.0) + ELBOW_REST_POWER;
if (joy1Btn(8))
wristPowers += WRIST_MAX_POWER;
else if (joy1Btn(6))
wristPowers -= WRIST_MAX_POWER;
if (joy1Btn(7))
deltaFingers = 1;
else if (joy1Btn(5))
deltaFingers = -1;
servo[lElbow] = 256-elbowPowers;
servo[rElbow] = elbowPowers;
//writeDebugStreamLine("%d",elbowPowers);
servo[lWrist] = 256-wristPowers;
servo[rWrist] = wristPowers;
servo[lFinger] += deltaFingers;
servo[rFinger] -= deltaFingers;*/
int deltaFingers = 0;
int rElbowPower = ELBOW_REST_POWER;
int lElbowPower = ELBOW_REST_POWER;
int rWristPower = WRIST_REST_POWER;
int lWristPower = WRIST_REST_POWER;
int rSholderPower = SHOLDER_REST_POWER;
int lSholderPower = SHOLDER_REST_POWER;
if (abs(joystick.joy1_y1) > JOYSTICK_THRESHOLD)
lSholderPower = joystick.joy1_y1 * SHOLDER_MAX_POWER / 128.0;
if (abs(joystick.joy1_y2) > JOYSTICK_THRESHOLD)
rSholderPower = joystick.joy1_y2 * SHOLDER_MAX_POWER / 128.0;
// Let joystick positions 7,0,1 trigger up
if (abs(joystick.joy1_TopHat) <= 1)
rElbowPower += ELBOW_MAX_POWER;
// Let joystick positions 3,4,5 trigger up
else if (abs(joystick.joy1_TopHat-4) <= 1)
rElbowPower -= ELBOW_MAX_POWER;
if (joy1Btn(4))
lElbowPower += ELBOW_MAX_POWER;
else if (joy1Btn(2))
lElbowPower -= ELBOW_MAX_POWER;
}
}