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
141
142
143
144
145
146
147
148
149
150
151
152
153
|
#include <main.h>
extern Gyro gyro;
#ifndef PI
#define PI 3.14159265L
#endif // PI
#define RPM_DEFAULT_MAX 160 // Max RPM of high-speed motor
#define RPM_CANNON_MAX 4000 // Max RPM of cannon (motor + gears)
#define RPM_CANNON_INC 31.49606299L // Expected change in RPM per joystick increment
#define RPM_THRESHOLD 100 // A margin of error for matching the target RPM
#define INC_FAULT 2 // Boost given to motors when left and right don't match RPM
#define INC_NORMAL 2 // Used to match actual velocity to the target
#define ROT_THRESHOLD (PI/8) // Margin of error for auto-aiming
#define ROT_SPEED 30 // How fast to rotate the cannon for aiming (for linear aiming only)
static double rpmTarget = 0; // Contains the desired RPM
static double rpmCannL = 0; // Left side's RPM
static double rpmCannR = 0; // Right side's RPM
static double rpmCannA = 0; // Average of the two above
static double rotCannon = 0; // Cumulative rotation of the cannon in radians
/**
* Calculates RPMs from cannon motors.
*
* There are two IMEs on the cannon, one per each side. This function converts the raw
* velocity reading from the motors and converts it to rotations per minute (RPM).
* Results are stored in global variables defined above, as well as an average of the
* two.
*/
void cannUpdate(void){
int cl,cr,rc;
/*
* Read raw velocity values.
*/
imeGetVelocity(ICANNONL,&cl);
imeGetVelocity(ICANNONR,&cr);
/*
* Divide by a divisor given in API.h and multiply the result by the cannon's gear ratio.
*/
rpmCannL = cl / 24.5L * 25.0L;
rpmCannR = cr / 24.5L * 25.0L;
/*
* Calculate an average of the two results from above.
*/
rpmCannA = (rpmCannL + rpmCannR) / 2;
/*
* Find the cumulative rotation of the cannon for auto-aiming.
*/
imeGet(IROTATER,&rc);
rotCannon = rc / 627.2L * 5;
rotCannon *= 2*PI; // convert to radians
}
/**
* Sets a target RPM for the cannon.
*
* This calculates a target RPM by simply multiplying the current position of a jostick axis
* by a predetermined increment that is 1/127th of the highest possible RPM of the cannon.
*
* @param a value returned from a call to joystickGetAnalog()
*/
void cannTarget(char jpos){
rpmTarget = jpos * RPM_CANNON_INC;
}
/**
* Runs/Updates the actual speed of the cannon motors.
*
* Speeds for each side of the cannon are kept inside the scope of this function. Two checks
* are made before applying the motor speeds. The first one will increase the speed of one
* side of the cannon if the RPMs of the two sides aren't reasonably equal. The second check
* will bring the overall speeds up or down depending on where the actual average RPM is
* compared to the target. Following these checks is the setting of the motor speeds.
*/
void cannRun(void){
static char speedCannL = 0;
static char speedCannR = 0;
/*
* Make sure cannon motors are up to about the same speed (as in RPM).
*/
if(rpmCannR < rpmTarget + RPM_THRESHOLD && rpmCannL < rpmCannR - RPM_THRESHOLD) speedCannL += INC_FAULT;
else if(rpmCannL < rpmTarget + RPM_THRESHOLD && rpmCannR < rpmCannL - RPM_THRESHOLD) speedCannR += INC_FAULT;
/*
* Adjust motor velocities to match the target.
*/
if(rpmTarget > rpmCannA + RPM_THRESHOLD){
speedCannL += INC_NORMAL;
speedCannR += INC_NORMAL;
}else if(rpmTarget < rpmCannA - RPM_THRESHOLD){
speedCannL -= INC_NORMAL;
speedCannR -= INC_NORMAL;
}
/*
* Apply the calculated motor speeds.
*/
motorSet(CANNON1,speedCannL);
motorSet(CANNON2,speedCannR);
motorSet(CANNON3,speedCannL);
motorSet(CANNON4,speedCannR);
}
/**
* Aims the cannon towards a specified location(?)
*
* ... . Also sets the motor speed.
*
* @param a rotation to base calculations of off (preferably that of the robot's body)
*/
void cannAim(double r){
static char speed = 0;
/*
* Do simple linear adjustments to have okay-ish aim. Once self-location is accomplished
* a more accurate function could be used to adjust aim.
*/
if(rotCannon > r + ROT_THRESHOLD) speed = ROT_SPEED;
else if(rotCannon < r - ROT_THRESHOLD) speed = -ROT_SPEED;
else speed = 0;
/*
* Apply the resulting speed to the motor.
*/
motorSet(ROTATER,speed);
}
|
