//control
void controlMov(motor393 *motor_){ //checa los botones de los motores
if(vexRT[ADELANTE_D]){//depende de que boton este presionado entonces mueve el motor de diferente forma
motorFront(&motor_[motorLeft]);
}
else if(vexRT[ATRAS_D]){
motorBack(&motor_[motorLeft]);
}
else {
motorStop(&motor_[motorLeft]);//en caso de que ningun boton esta presionado, entonces apaga el motor
}
if(vexRT[ADELANTE_I]){ //lo mismo que arriba pero con el boton izquierdo
motorFront(&motor_[motorRight]);
}
else if(vexRT[ATRAS_I]){
motorBack(&motor_[motorRight]);
}
else {
motorStop(&motor_[motorRight]);
}
if(vexRT[IZQUIERDA]){ //lo mismo que arriba pero con el boton izquierdo
motorFront(&motor_[motorOmni]);
}
else if(vexRT[DERECHA]){
motorBack(&motor_[motorOmni]);
}
else {
motorStop(&motor_[motorOmni]);
}
}
bool dataHandler(uint8_t *dataRecv, uint8_t *dataSend, uint8_t *dataSendLen)
{
TransPacket *dataPack = (TransPacket *)dataRecv;
TransPacket *dataHandle = (TransPacket *)dataSend;
if(dataPack->header != PC_HEADER)
return false;
if(checkXor(&dataPack->dataBuf[0], dataPack->len, &dataPack->dataBuf[dataPack->len]))
return false;
dataHandle->header = dataPack->header;
switch(dataPack->cmd)
{
case 0x01:
dataHandle->len = 0;
case 0x02:
commanderGetPacket(dataPack);
break;
case 0x03:
velocityGetPID(dataPack);
break;
case 0x04:
turnGetPID(dataPack);
break;
case 0x10:
accBack(dataHandle);
break;
case 0x11:
gyroBack(dataHandle);
break;
case 0x12:
magBack(dataHandle);
break;
case 0x13:
eulerBack(dataHandle);
break;
case 0x14:
motorBack(dataHandle);
break;
case 0x15:
// velocityPIDBack(dataHandle);
break;
case 0x16:
turnPIDBack(dataHandle);
break;
}
generateXor(&dataHandle->dataBuf[0], dataHandle->len, &dataHandle->dataBuf[dataHandle->len]);
*dataSendLen = dataHandle->len + 5;
return true;
}
void controlBandas(motor393 *motor_){ //funcion que controla las bandas
if(vexRT[SUCCIONAR]){
motorFront(&motor_[motorSuc]);
}
else if(vexRT[ESCUPIR]){
motorBack(&motor_[motorSuc]);
}
else if(vexRT[APAGARBANDA]){
motorStop(&motor_[motorSuc]);
}
}
void down(motor393 *motor1, motor393 *motor2){
motorBack(motor1);
motorBack(motor2);
}
void turnback(motor393 *motor_){
motorBack(&motor_[motorRight]);
motorStop(&motor_[motorLeft]);
delay(300);
}
void back(motor393 *motor_){
motorBack(&motor_[motorRight]);
motorBack(&motor_[motorLeft]);
}
