void Motor_Init(){
#ifdef MOTOR1A_ANSEL
MOTOR1A_ANSEL = 0;
#endif
#ifdef MOTOR2A_ANSEL
MOTOR2A_ANSEL = 0;
#endif
#ifdef MOTOR1B_ANSEL
MOTOR1B_ANSEL = 0;
#endif
#ifdef MOTOR2B_ANSEL
MOTOR2B_ANSEL = 0;
#endif
Motor_Disable();
Motor_Set1Forward();
Motor_Set2Forward();
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
TMR2 = 0; // Set Timer 2 to Zero
PR2 = 10230; // Set Period
T2CONbits.TCKPS = 0; // Set Prescale of 1
OC2CONbits.OCM = 6; // PWM Mode on OC, Fault Pin Disabled
OC4CONbits.OCM = 6; // PWM Mode on OC, Fault Pin Disabled
OC2CONbits.OCTSEL = 0; // Use Timer 2
OC4CONbits.OCTSEL = 0; // Use Timer 2
T2CONbits.ON = 1; // Enable Timer 2
OC2CONbits.ON = 1; // Enable Output Compare 2
OC4CONbits.ON = 1; // Enable Output Compare 4
}
void motorTest(SmartCar * smartCar) {
int16_t speed = 300;
Motor_Enable(&smartCar->motor);
Servo_init(&smartCar->servo);
Servo_runAs(&smartCar->servo,0);
while (1) {
Servo_runAs(&smartCar->servo,0);
smartCar->motor.sendPID = 1;//for bluetooth send start
Motor_runAs(&smartCar->motor, speed);
Segment_print(&smartCar->segment[0], smartCar->motor.targetSpeed);
Segment_print(&smartCar->segment[1], Encoder_get(&smartCar->encoder));
Segment_print(&smartCar->segment[2], smartCar->motor.currentSpeed);
switch (board.button.check()) {
case 1: // fast
if (speed < 2000) {
speed = speed + 50;
} else {
speed = 2000;
}
break;
case 2: // slow
if (speed > -2000) {
speed = speed - 50;
} else {
speed = -2000;
}
break;
case 3: // reverse
speed = -speed;
break;
case 4: //motor test end
smartCar->motor.sendPID = 0;//for bluetooth send stop
speed = 0;
Motor_Disable(&smartCar->motor);
Segment_print(&smartCar->segment[0], speed);
Segment_print(&smartCar->segment[1], speed);
return;
}
}
}
void main(void){
SYSTEMConfigPerformance(10000000);
STATE state = START;
unsigned short int lineCount = 0;
bool ignoreTurns = false;
// Initialize Motor(s))
Motor_Init();
// Initialize IRSensor(s)
IRSensor_Init();
// LCD Initialize
LCD_Init();
while(1){
printDebug(state);
switch(state){
case START:
Motor_Disable();
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL))state = TRAVEL;
break;
case TRAVEL:
Motor_Enable();
if(IRSensor_CheckLeftFront(200)){
state = TURN_LEFT;break;
}else if(IRSensor_CheckRightFront(200)){
state = TURN_RIGHT;break;
}else if(IRSensor_GetCenterLeft() < 300 || IRSensor_GetCenterRight < 300){
if(IRSensor_GetCenterLeft() > IRSensor_GetCenterRight()){
Motor_Set1DutyCycle(0);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
}else{
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(0);
};
}else if(IRSensor_GetCenterLeft() != IRSensor_GetCenterRight()){
float biasValue = 1.0 + abs(IRSensor_GetCenterLeft() - IRSensor_GetCenterRight()) / 2000.0;
if(IRSensor_GetCenterLeft() > IRSensor_GetCenterRight()){
Motor_Set1DutyCycle(biasValue * STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(1.0/biasValue * STANDARD_DUTY_CYCLE);
}else{
Motor_Set1DutyCycle(1.0/biasValue * STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(biasValue * STANDARD_DUTY_CYCLE);
};
}else{
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
};
break;
case TURN_LEFT:
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Disable();
state = TRAVEL;
}else{
Motor_Set1DutyCycle(0.0);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
Motor_Enable();
};
break;
case TURN_RIGHT:
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Disable();
state = TRAVEL;
}else{
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
Motor_Set2DutyCycle(0.0);
Motor_Enable();
};
break;
};
};
};
void oldmain(void){
SYSTEMConfigPerformance(10000000);
STATE state = START;
unsigned short int lineCount = 0;
bool ignoreTurns = false;
// Initialize Motor(s))
Motor_Init();
// Initialize IRSensor(s)
IRSensor_Init();
// LCD Initialize
LCD_Init();
Motor_Enable();
while(1){
printDebug(state);
switch(state){
case START:
Motor_Disable();
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL))state = TRAVEL;
break;
case TURN_LEFT:
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Disable();
state = TRAVEL;
}else{
Motor_Set1DutyCycle(0.0);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
Motor_Enable();
};
break;
case TURN_RIGHT:
if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Disable();
state = TRAVEL;
}else{
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
Motor_Set2DutyCycle(0.0);
Motor_Enable();
};
break;
case TRAVEL:
Motor_Set1Forward();
Motor_Set2Forward();
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Enable();
if(!IRSensor_CheckCenterRight(IR_TRIGGER_ADJUST) && IRSensor_CheckCenterLeft(IR_TRIGGER_ADJUST)){
Motor_Disable();
state = ADJUST_RIGHT;break;
}else if(!IRSensor_CheckCenterLeft(IR_TRIGGER_ADJUST) && IRSensor_CheckCenterRight(IR_TRIGGER_ADJUST)){
Motor_Disable();
state = ADJUST_LEFT;break;};
// if(IRSensor_CheckFront()){
// lineCount++;
// if(lineCount == 1){// Entering Extra Credit 'T' Intersection
// state = TURN_LEFT;
// }else if(lineCount == 2){// Exiting Extra Credit 'T' Intersection
// state = TURN_RIGHT;
// }else if(lineCount == 5){// End of First Time Through
// state = TURN_AROUND;
// };
//};
if(!ignoreTurns && IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){Motor_Disable();state = TURN_LEFT;break;};
if(!ignoreTurns && IRSensor_CheckRightFront(IR_TRIGGER_TURN)){Motor_Disable();state = TURN_RIGHT;break;};//
break;
case ADJUST_LEFT:
Motor_Set1DutyCycle(MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(0.75 * MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
Motor_Enable();
if(IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){
state = TURN_LEFT;
break;
}else if(IRSensor_CheckRightFront(IR_TRIGGER_TURN)){
state = TURN_RIGHT;
break;
}else if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
state = TRAVEL;
break;
}else{
break;
};
case ADJUST_RIGHT:
Motor_Set1DutyCycle(0.75 * MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
Motor_Enable();
if(IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){
state = TURN_LEFT;
break;
}else if(IRSensor_CheckRightFront(IR_TRIGGER_TURN)){
state = TURN_RIGHT;
break;
}else if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
state = TRAVEL;
break;
}else{
break;
};
//case TURN_AROUND:
// while(IRSensor_CheckCenterLeft(IR_TRIGGER_TRAVEL)){
// Motor_Set1Backward();
// Motor_Set2Forward();
// };
// delayMs(IR_DELAY_TURNAROUND);
// state = TRAVEL;
// ignoreTurns = true;
// break;
//case END:
// Motor_Disable();
// break;
};
};
};