void Accelerometer::setMultiplier(int multi)
{
if(sensorport != 13) {
analogCalibrate(sensorport);
reset();
pollSensor();
}
}
LightSensor::LightSensor(int SensorPort)
{
if(checkAnalogPort(SensorPort)) {
sensorport = SensorPort;
analogCalibrate(SensorPort); }
else
sensorport = 13;
curval = 0;
}
/*
* Call this from the default Initialize function.
* After, be sure to reinitialize each motor you will be using on your robot.
*/
void riceBotInitialize() {
Motor *MOTDTFrontRight = newMotor();
Motor *MOTDTFrontMidRight = newMotor();
Motor *MOTDTMidRight = newMotor();
Motor *MOTDTBackRight = newMotor();
Motor *MOTDTFrontLeft = newMotor();
Motor *MOTDTFrontMidLeft = newMotor();
Motor *MOTDTMidLeft = newMotor();
Motor *MOTDTBackLeft = newMotor();
Motor *MOTARMFront = newMotor();
Motor *MOTARMRight = newMotor();
Motor *MOTARMLeft = newMotor();
Motor *MOTARMTopRight = newMotor();
Motor *MOTARMBottomRight = newMotor();
Motor *MOTARMTopLeft = newMotor();
Motor *MOTARMBottomLeft = newMotor();
Motor *MOTCOL = newMotor();
printf("%d\n\r", imeInitializeAll());
imeReset(IMEARMLEFT);
imeReset(IMEARMRIGHT);
encDTLeft = encoderInit(0, 0, false);
encDTRight = encoderInit(0, 0, false);
encARMLeft = encoderInit(0, 0, false);
encARMRight = encoderInit(0, 0, false);
Ricencoder *ENCDT = newEncoder();
Ricencoder *ENCARM = newEncoder();
// gyro = gyroInit(0, 196);
// gyroVal = gyroGet(gyro);
analogCalibrate(POTARMLeft);
analogCalibrate(POTARMRight);
analogCalibrate(POTARMFront);
Pid *PidARMLeft = newPid();
Pid *PidARMRight = newPid();
Pid *PidARMFront = newPid();
}
Accelerometer::Accelerometer(int SensorPort)
{
if(checkAnalogPort(SensorPort))
sensorport = SensorPort;
else
sensorport = 13;
if(sensorport != 13)
analogCalibrate(sensorport);
curval = 0;
multipier = 0;
}
void writeKFValues() {
Kalman filterx = filterInit(1);//Create Kalman Object X
//Kalman filtery = filterInit(2);//Create Kalman Object Y
analogCalibrate(1);//Calibrate Analog Port
file = fopen("kf.csv", "w");
while(1){
//accelRead(KalmanObject)
fprintf(file, "%d,%d\n", analogReadCalibrated(1), accelRead(filterx));
printf("RawX: %d \t FilteredX: %d\n\r",analogReadCalibrated(1),accelRead(filterx));
//printf("RawY: %d\t FilteredY: %d\n\r",analogReadCalibrated(2),accelRead(filtery));
delay(2);
}
}
// RobotMode robotMode;
void initialize() {
//gyro = gyroInit(1, 0);
// robotMode = STANDARD_MODE;
analogCalibrate(3);
claw.status = HOLDING;
claw.holdTarget = clawGetPosition();
claw.autoOpenPos = 0;
claw.autoOpenTrigger -1;
taskCreate(clawTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
taskCreate(armTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
leftEncoder = encoderInit(LEFT_ENC_1,LEFT_ENC_2,1);
rightEncoder = encoderInit(RIGHT_ENC_1,RIGHT_ENC_2,0);
}
void LineTracker::calibrate()
{
if(sensorport != 13)
analogCalibrate(sensorport);
}
void LightSensor::calibrate()
{
if(sensorport != 13)
analogCalibrate(sensorport);
}
