/
chiller.cpp
42 lines (38 loc) · 1.32 KB
/
chiller.cpp
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
#include "chiller.h"
#include "drivefunctions.h"
#include "constants.h"
#define DISTANCE_1 20
#define DISTANCE_2 7
#define DISTANCE_3 12
/* CHILLER
*==========
*Takes robot from shop floor to putting the skid in chiller */
Chiller::Chiller()
{
}
void Chiller::Run() {
//Turn and drive to hit wall in shop
turnToRPSHeading(90, TURN_POWER, RIGHT, true, 1);
Sleep(250);
turnUntilRPSHeading(90, TURN_POWER);
driveUntilSwitchPress(-1 * FORWARD_POWER, BACK_SWITCH, 20);
//Drive away from wall and turn
driveToRPSCoordinate(RPS_POWER, CHILLER_X, false, true, 90);
turnToRPSHeading(0, TURN_POWER, LEFT, true, 1);
turn(true, TURN_POWER, 10, true);
Sleep(250);
turnToRPSHeading(0, TURN_POWER, CLOSEST, true, 1);
turnUntilRPSHeading(0, RPS_POWER);
driveUntilSwitchPress(-1 * FORWARD_POWER, BACK_SWITCH, 10);
//Set forklift to horizontal to drop off skid
forklift.SetDegree(DROP_OFF_ANGLE);
//Drive to chiller
drive(SCOOP_POWER, DISTANCE_1, false, false, 1.1);
drive(FORWARD_POWER * -1, DISTANCE_2, false, false);
//Raise forklift a little bit and go in again to push skid in
forklift.SetDegree(SKID_PUSH_ANGLE);
drive(FORWARD_POWER, DISTANCE_3, false, false);
Sleep(50);
turnUntilRPSHeading(0, TURN_POWER, 2.5);
forklift.SetDegree(OUT_OF_CHILLER_ANGLE);
}