void stopElevator() {
halt = true;
motorStop();
}
void receive_pi_command(){
for(i = 0; i < 3; i++){
pos_received_x[i] = pi_received_string[i];
pos_received_y[i] = pi_received_string[i+3];
}
if(pos_received_x[0]=='0'){
pos_received_x1[0] = pos_received_x[1];
pos_received_x1[1] = pos_received_x[2];
pts_x = atoi(pos_received_x1);
motorStop();
}
else
pts_x = atoi(pos_received_x);
if(pos_received_y[0]=='0'){
pos_received_y1[0] = pos_received_y[1];
pos_received_y1[1] = pos_received_y[2];
pts_y = atoi(pos_received_y1);
motorStop();
}
else
pts_y = atoi(pos_received_y);
//USART_puts(USART6, "\r\nPi receive test\n\r");
// pts_x is the position on image plane
// actual position of object
// pts_x = XXX * pts_x
// degree of lidar data
lidar_Pos = pts_x*9/32;
//motorForward();
object_distance = Lidar_distance[lidar_Pos];
#if 0
USART_puts(USART6, "\r\nX:");
USART_putd(USART6, pts_x);
USART_puts(USART6, " Y:");
USART_putd(USART6, pts_y);
USART_puts(USART6, "\r\nlidar_Pos:");
USART_putd(USART6, lidar_Pos);
USART_puts(USART6, "\r\nobject_distance:");
USART_putd(USART6, object_distance);
#endif
#if 1
// Motor moving
if(pts_x < 320){
// motorLeft(lValue, rValue)
Left();
//vTaskDelay(300);
}
else
{
Right();
//vTaskDelay(300);
}
//motorStop();
//vTaskDelay(10);
#endif
for(i = 0; i < 3; i++){
pos_received_x[i] = 0;
pos_received_y[i] = 0;
}
pts_x = 0;
pts_y = 0;
}
void stopIntake()
{
motorStop (motor5);
motorStop (motor6);
}
void Robot::halt() {
motorStop(LEFTWHEELMOTOR);
motorStop(RIGHTWHEELMOTOR);
moving = false;
printMotorPositions();
}
static void motorHandleOneArg(const char *myarg_1)
{
const char *myarg = myarg_1;
int iValue = 0;
double fValue = 0;
int motor_axis_no = 0;
int nvals = 0;
/* ADSPORT= */
if (!strncmp(myarg_1, ADSPORT_equals_str, strlen(ADSPORT_equals_str))) {
int err_code;
myarg_1 += strlen(ADSPORT_equals_str);
err_code = motorHandleADS_ADR(myarg_1);
if (err_code == -1) return;
if (err_code == 0) {
cmd_buf_printf("OK");
return;
}
RETURN_OR_DIE("%s/%s:%d myarg_1=%s err_code=%d",
__FILE__, __FUNCTION__, __LINE__,
myarg_1,
err_code);
}
/* Main.*/
if (!strncmp(myarg_1, Main_dot_str, strlen(Main_dot_str))) {
myarg_1 += strlen(Main_dot_str);
}
/* From here on, only M1. commands */
/* e.g. M1.nCommand=3 */
nvals = sscanf(myarg_1, "M%d.", &motor_axis_no);
if (nvals != 1) {
RETURN_OR_DIE("%s/%s:%d line=%s nvals=%d",
__FILE__, __FUNCTION__, __LINE__,
myarg, nvals);
}
AXIS_CHECK_RETURN(motor_axis_no);
myarg_1 = strchr(myarg_1, '.');
if (!myarg_1) {
RETURN_OR_DIE("%s/%s:%d line=%s missing '.'",
__FILE__, __FUNCTION__, __LINE__,
myarg);
}
myarg_1++; /* Jump over '.' */
if (0 == strcmp(myarg_1, "bBusy?")) {
cmd_buf_printf("%d", isMotorMoving(motor_axis_no));
return;
}
/* bError? */
if (!strcmp(myarg_1, "bError?")) {
cmd_buf_printf("%d", get_bError(motor_axis_no));
return;
}
/* bEnable? bEnabled? Both are the same in the simulator */
if (!strcmp(myarg_1, "bEnable?") || !strcmp(myarg_1, "bEnabled?")) {
cmd_buf_printf("%d",getAmplifierOn(motor_axis_no));
return;
}
/* bExecute? */
if (!strcmp(myarg_1, "bExecute?")) {
cmd_buf_printf("%d", cmd_Motor_cmd[motor_axis_no].bExecute);
return;
}
/* bHomeSensor? */
if (0 == strcmp(myarg_1, "bHomeSensor?")) {
cmd_buf_printf("%d", getAxisHome(motor_axis_no));
return;
}
/* bLimitBwd? */
if (0 == strcmp(myarg_1, "bLimitBwd?")) {
cmd_buf_printf("%d", getNegLimitSwitch(motor_axis_no) ? 0 : 1);
return;
}
/* bLimitFwd? */
if (0 == strcmp(myarg_1, "bLimitFwd?")) {
cmd_buf_printf("%d", getPosLimitSwitch(motor_axis_no) ? 0 : 1);
return;
}
/* bHomed? */
if (0 == strcmp(myarg_1, "bHomed?")) {
cmd_buf_printf("%d", getAxisHomed(motor_axis_no) ? 1 : 0);
return;
}
/* bReset? */
if (!strcmp(myarg_1, "bReset?")) {
cmd_buf_printf("%d",cmd_Motor_cmd[motor_axis_no].bReset);
return;
}
/* fAcceleration? */
if (0 == strcmp(myarg_1, "fAcceleration?")) {
cmd_buf_printf("%g", cmd_Motor_cmd[motor_axis_no].acceleration);
return;
}
/* fActPosition? */
if (0 == strcmp(myarg_1, "fActPosition?")) {
cmd_buf_printf("%g", getMotorPos(motor_axis_no));
return;
}
/* fActVelocity? */
if (0 == strcmp(myarg_1, "fActVelocity?")) {
cmd_buf_printf("%g", getMotorVelocity(motor_axis_no));
return;
}
/* fPosition? */
if (0 == strcmp(myarg_1, "fPosition?")) {
/* The "set" value */
cmd_buf_printf("%g", cmd_Motor_cmd[motor_axis_no].position);
return;
}
/* nCommand? */
if (0 == strcmp(myarg_1, "nCommand?")) {
cmd_buf_printf("%d", cmd_Motor_cmd[motor_axis_no].command_no);
return;
}
/* nMotionAxisID? */
if (0 == strcmp(myarg_1, "nMotionAxisID?")) {
/* The NC axis id is the same as motion axis id */
printf("%s/%s:%d %s(%d)\n", __FILE__, __FUNCTION__, __LINE__,
myarg_1, motor_axis_no);
cmd_buf_printf("%d", motor_axis_no);
return;
}
/* stAxisStatus? */
if (0 == strcmp(myarg_1, "stAxisStatus?")) {
/* getMotorPos must be first, it calls
simulateMotion() */
double fActPostion = getMotorPos(motor_axis_no);
int bEnable = getAmplifierOn(motor_axis_no);
int bReset = 0;
int bExecute = cmd_Motor_cmd[motor_axis_no].bExecute;
unsigned nCommand = 0;
unsigned nCmdData = cmd_Motor_cmd[motor_axis_no].nCmdData;
double fVelocity = 0;
double fPosition = 0;
double fAcceleration = 0;
double fDecceleration = 0;
int bJogFwd = 0;
int bJogBwd = 0;
int bLimitFwd = getPosLimitSwitch(motor_axis_no) ? 0 : 1;
int bLimitBwd = getNegLimitSwitch(motor_axis_no) ? 0 : 1;
double fOverride = 0;
int bHomeSensor = getAxisHome(motor_axis_no);
int bEnabled = bEnable;
int bError = get_bError(motor_axis_no);
int nErrorId = get_nErrorId(motor_axis_no);
double fActVelocity = getMotorVelocity(motor_axis_no);
double fActDiff = 0;
int bHomed = getAxisHomed(motor_axis_no);
int bBusy = isMotorMoving(motor_axis_no);
cmd_buf_printf("Main.M%d.stAxisStatus="
"%d,%d,%d,%u,%u,%g,%g,%g,%g,%d,"
"%d,%d,%d,%g,%d,%d,%d,%u,%g,%g,%g,%d,%d",
motor_axis_no,
bEnable, /* 1 */
bReset, /* 2 */
bExecute, /* 3 */
nCommand, /* 4 */
nCmdData, /* 5 */
fVelocity, /* 6 */
fPosition, /* 7 */
fAcceleration, /* 8 */
fDecceleration, /* 9 */
bJogFwd, /* 10 */
bJogBwd, /* 11 */
bLimitFwd, /* 12 */
bLimitBwd, /* 13 */
fOverride, /* 14 */
bHomeSensor, /* 15 */
bEnabled, /* 16 */
bError, /* 17 */
nErrorId, /* 18 */
fActVelocity, /* 19 */
fActPostion, /* 20 */
fActDiff, /* 21 */
bHomed, /* 22 */
bBusy /* 23 */
);
return;
}
/* sErrorMessage? */
if (!strcmp(myarg_1, "sErrorMessage?")) {
char buf[32]; /* 9 should be OK */
int nErrorId = get_nErrorId(motor_axis_no);
snprintf(buf, sizeof(buf), "%x", nErrorId);
cmd_buf_printf("%s", buf);
return;
}
/* End of "get" commands, from here, set commands */
/* nCommand=3 */
nvals = sscanf(myarg_1, "nCommand=%d", &iValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].command_no = iValue;
cmd_buf_printf("OK");
return;
}
/* nCmdData=1 */
nvals = sscanf(myarg_1, "nCmdData=%d", &iValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].nCmdData = iValue;
cmd_buf_printf("OK");
return;
}
/* fPosition=100 */
nvals = sscanf(myarg_1, "fPosition=%lf", &fValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].position = fValue;
cmd_buf_printf("OK");
return;
}
/* fHomePosition */
nvals = sscanf(myarg_1, "fHomePosition=%lf", &fValue);
if (nvals == 1) {
/* Do noting */
cmd_buf_printf("OK");
return;
}
/* fVelocity=20 */
nvals = sscanf(myarg_1, "fVelocity=%lf", &fValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].velocity = fValue;
cmd_buf_printf("OK");
return;
}
/* fAcceleration=1000 */
nvals = sscanf(myarg_1, "fAcceleration=%lf", &fValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].acceleration = fValue;
cmd_buf_printf("OK");
return;
}
/* bEnable= */
nvals = sscanf(myarg_1, "bEnable=%d", &iValue);
if (nvals == 1) {
setAmplifierPercent(motor_axis_no, iValue ? 100 : 0);
cmd_buf_printf("OK");
return;
}
/* bExecute= */
nvals = sscanf(myarg_1, "bExecute=%d", &iValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].bExecute = iValue;
if (!iValue) {
/* bExecute=0 is always allowed, regardless the command */
motorStop(motor_axis_no);
cmd_buf_printf("OK");
return;
} else if (iValue == 1) {
if (cmd_Motor_cmd[motor_axis_no].velocity >
cmd_Motor_cmd[motor_axis_no].maximumVelocity) {
fprintf(stdlog, "%s/%s:%d axis_no=%d velocity=%g maximumVelocity=%g\n",
__FILE__, __FUNCTION__, __LINE__,
motor_axis_no,
cmd_Motor_cmd[motor_axis_no].velocity,
cmd_Motor_cmd[motor_axis_no].maximumVelocity);
set_nErrorId(motor_axis_no, 0x512);
cmd_buf_printf("OK");
return;
}
switch (cmd_Motor_cmd[motor_axis_no].command_no) {
case 1:
{
int direction = 1;
if (cmd_Motor_cmd[motor_axis_no].velocity < 0) {
direction = 0;
cmd_Motor_cmd[motor_axis_no].velocity = -cmd_Motor_cmd[motor_axis_no].velocity;
}
(void)moveVelocity(motor_axis_no,
direction,
cmd_Motor_cmd[motor_axis_no].velocity,
cmd_Motor_cmd[motor_axis_no].acceleration);
cmd_buf_printf("OK");
}
break;
case 2:
(void)movePosition(motor_axis_no,
cmd_Motor_cmd[motor_axis_no].position,
1, /* int relative, */
cmd_Motor_cmd[motor_axis_no].velocity,
cmd_Motor_cmd[motor_axis_no].acceleration);
cmd_buf_printf("OK");
break;
case 3:
(void)movePosition(motor_axis_no,
cmd_Motor_cmd[motor_axis_no].position,
0, /* int relative, */
cmd_Motor_cmd[motor_axis_no].velocity,
cmd_Motor_cmd[motor_axis_no].acceleration);
cmd_buf_printf("OK");
break;
case 10:
{
if (cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor &&
cmd_Motor_cmd[motor_axis_no].homeVeloFromHomeSensor) {
(void)moveHomeProc(motor_axis_no,
0, /* direction, */
cmd_Motor_cmd[motor_axis_no].nCmdData,
cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor,
cmd_Motor_cmd[motor_axis_no].acceleration);
cmd_buf_printf("OK");
} else {
cmd_buf_printf("Error : %d %g %g",
70000,
cmd_Motor_cmd[motor_axis_no].homeVeloTowardsHomeSensor,
cmd_Motor_cmd[motor_axis_no].homeVeloFromHomeSensor);
}
}
break;
default:
RETURN_OR_DIE("%s/%s:%d line=%s command_no=%u",
__FILE__, __FUNCTION__, __LINE__,
myarg, cmd_Motor_cmd[motor_axis_no].command_no);
}
return;
}
RETURN_OR_DIE("%s/%s:%d line=%s invalid_iValue=%u '.'",
__FILE__, __FUNCTION__, __LINE__,
myarg, iValue);
}
/* bReset= */
nvals = sscanf(myarg_1, "bReset=%d", &iValue);
if (nvals == 1) {
cmd_Motor_cmd[motor_axis_no].bReset = iValue;
if (iValue) {
motorStop(motor_axis_no);
set_nErrorId(motor_axis_no, 0);
set_bError(motor_axis_no, 0);
}
cmd_buf_printf("OK");
return;
}
/* if we come here, we do not understand the command */
RETURN_OR_DIE("%s/%s:%d line=%s",
__FILE__, __FUNCTION__, __LINE__,
myarg);
}
void stopIntake() {
motorStop(MOTOR5);
motorStop(MOTOR6);
}
void stopIntake()
{
motorStop (MOTOR_RIGHT_LINE_INTAKE);
motorStop (MOTOR_LEFT_LINE_INTAKE);
}
void stopRobot(motor393 *motor_){
motorStop(&motor_[motorRight]);
motorStop(&motor_[motorLeft]);
}
void MIYbot::spin(int duration, boolean reverse){
MotorRun(2,SLOW,reverse);
MotorRun(3,SLOW,! reverse);
delay(duration);
motorStop();
}
void handleLcdInput(unsigned int input)
{
switch(currentPage)
{
case(startLink)://Link Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startFPS;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startBattery;
break;
}
case(LCD_BTN_CENTER):
{
//check link status
//if connected go to connected
//else go to not connected
if (isJoystickConnected(1))
{
currentPage = startLink + 1;
}
else
{
currentPage = startLink + 2;
}
break;
}
}
break;
}
case(startBattery)://Battery Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startLink;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startSensor;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery + 1;
break;
}
}
break;
}
case(startSensor)://Sensor Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startBattery;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor;
break;
}
case(LCD_BTN_CENTER):
{
quadNum = 0;
imeNum = 0;
sensIndex = 1;
currentPage = startSensor + 1;
break;
}
}
break;
}
case(startMotor)://Motor Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startSensor;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startAuton;
break;
}
case(LCD_BTN_CENTER):
{
currentMotorNum = 1;
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startAuton)://Auton Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startMotor;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMode;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startAuton + 1;
break;
}
}
break;
}
case(startMode)://Op Mode status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startAuton;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startFPS;;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMode + 1;
break;
}
}
break;
}
case(startFPS):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startMode;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startLink;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startFPS + 1;
break;
}
}
break;
}
case(startLink + 1)://Link Status - Link Est.
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
//returns to top of branch
currentPage = startLink;
break;
}
}
break;
}
case(startLink + 2)://Link Status - Link Not Est.
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
//return to top of branch
currentPage = startLink;
break;
}
}
break;
}
case(startBattery + 1)://Battery Status - Battery Selector
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentPage = startBattery + 2;
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startBattery + 3;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startBattery + 2)://Battery Status - Battery Selector - Prims Battery Status
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startBattery + 3)://Battery Status - Battery Selector - Backup
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//nothing
break;
}
case(LCD_BTN_RIGHT):
{
//nothing
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startBattery;
break;
}
}
break;
}
case(startSensor + 1):
{
/*
* SENSOR SELECTION MENU
*
* Menu for selecting individual types of sensors such
* which can then have individual sensors. This screen
* will display the name of the Sensor Group along with
* the Buttons to select the sensor group and iterate
* between individual sensor groups.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous Sensor Group
* CENTER BUTTON: Select Current Sensor Group
* RIGHT BUTTON: Next Sensor Group
*
*/
case(LCD_BTN_LEFT):
{
sensIndex--;
if(sensIndex < 1){
sensIndex = 4;
}
break;
}
case(LCD_BTN_RIGHT):
{
sensIndex++;
if(sensIndex > 4){
sensIndex = 1;
}
break;
}
case(LCD_BTN_CENTER):
{
//the +1 must be added to go beyond this page(startSensor + 1)
currentPage = startSensor + sensIndex + 1;
break;
}
}
break;
}
case(startSensor + 2):
{
/*
* QUADRATURE: Button Control
*
* There can be multiple Quadratures meaning the
* case has functions to iterate between the
* individual Quadratures. The screen will display
* both the data of the current Quadrature and
* the buttons to iterate between the other
* individual Quadratures.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous Quadrature
* CENTER BUTTON: Return to Main Page
* RIGHT BUTTON: Next Quadrature
*
*/
case(LCD_BTN_LEFT):
{
quadNum--;
if(quadNum < 0)
quadNum = 3;
break;
}
case(LCD_BTN_RIGHT):
{
quadNum++;
if(quadNum > 3)
quadNum = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 3):
{
/*
* IME: Button Control
*
* There can be multiple IME's meaning the IME
* case has functions to iterate between the
* individual IME's. The screen will display
* both the data and the buttons to iterate
* between the individual IME's.
*
*/
switch(input)
{
/*
* LEFT BUTTON: Previous IME
* CENTER BUTTON: Return to Main Page
* RIGHT BUTTON: Next IME
*
*/
case(LCD_BTN_LEFT):
{
imeNum--;
if(imeNum < 0)
imeNum = 1;
break;
}
case(LCD_BTN_RIGHT):
{
imeNum++;
if(imeNum > 1)
imeNum = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 4):
{
/*
* GYROSCOPE: Button Control
*
* There will only be one Gyroscope at all times,
* meaning the side buttons will have to do nothing.
* This results in the only usage of the middle
* button to be returning to the main Sensor screen.
*
*/
switch(input)
{
/*
* LEFT BUTTON: No Function
* CENTER BUTTON: Return to aMain Page
* RIGHT BUTTON: No Function
*
*/
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startSensor + 5):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startSensor;
break;
}
}
break;
}
case(startMotor + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupNum = 0;
currentPage = startMotor + 2;
break;
}
case(LCD_BTN_RIGHT):
{
currentMotorNum = 1;
currentPage = startMotor + 9;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor;
break;
}
}
break;
}
case(startMotor + 2):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupNum--;
if(currentGroupNum < 0)
{
currentGroupNum = NUM_GROUPS - 1;
}
//currentPage = startMotor + 2;
//stay on to this page
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupNum++;
if(currentGroupNum == NUM_GROUPS)
{
currentGroupNum = 0;
}
//currentPage = startMotor + 2;
//stay on to this page
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 3):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
/*int avg = 0;
int counter = 0;
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
avg += motorGet(groups[currentGroupNum].motors[i]);
counter++;
}
if(counter != 0)
{
avg /= counter;
}
currentGroupSpeed = avg;*/
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupIndex = 0;
currentPage = startMotor + 6;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startMotor + 4):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorStop(groups[currentGroupNum].motors[i]);
}
//currentGroupSpeed = 0;
//currentPage = startMotor + 4;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 5;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 5):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorSet(groups[currentGroupNum].motors[i], -127);
}
//currentGroupSpeed = -127;
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_RIGHT):
{
for (int i = 0; i < groups[currentGroupNum].validMotors; i++)
{
motorSet(groups[currentGroupNum].motors[i], 127);
}
//currentGroupSpeed = 127;
currentPage = startMotor + 4;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 4;
break;
}
}
break;
}
case(startMotor + 6):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentGroupIndex--;
if (currentGroupIndex < 0)
{
currentGroupIndex = groups[currentGroupNum].validMotors - 1;
}
//currentPage = startMotor + 6;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentGroupIndex++;
if (currentGroupIndex == groups[currentGroupNum].validMotors)
{
currentGroupIndex = 0;
}
//currentPage = startMotor + 6;
//stay on this page
break;
}
case(LCD_BTN_CENTER):
{
//currentGroupSpeed = 0;
currentPage = startMotor + 7;
break;
}
}
break;
}
case(startMotor + 7):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorStop(groups[currentGroupNum].motors[currentGroupIndex]);
//currentGroupSpeed = 0;
//currentPage = startMotor + 7;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 8;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 3;
break;
}
}
break;
}
case(startMotor + 8):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorSet(groups[currentGroupNum].motors[currentGroupIndex], -127);
currentPage = startMotor + 7;
break;
}
case(LCD_BTN_RIGHT):
{
motorSet(groups[currentGroupNum].motors[currentGroupIndex], 127);
currentPage = startMotor + 7;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 7;
break;
}
}
break;
}
case(startMotor + 9):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
currentMotorNum--;
if(currentMotorNum < 1)
{
currentMotorNum = 10;
}
//currentPage = startMotor + 9
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentMotorNum++;
if(currentMotorNum > 10)
{
currentMotorNum = 1;
}
//currentPage = startMotor + 9
//stay on this page
break;
}
case(LCD_BTN_CENTER):
{
currentMotorSpeed = motorGet(currentMotorNum);
currentPage = startMotor + 10;
break;
}
}
break;
}
case(startMotor + 10):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
motorStop(currentMotorNum);
//currentMotorSpeed = 0;
//currentPage = startMotor + 10;
//stay on this page
break;
}
case(LCD_BTN_RIGHT):
{
currentPage = startMotor + 11;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 1;
break;
}
}
break;
}
case(startMotor + 11):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
//currentMotorSpeed = -127;
motorSet(currentMotorNum, -127);
currentPage = startMotor + 10;
break;
}
case(LCD_BTN_RIGHT):
{
//currentMotorSpeed = 127;
motorSet(currentMotorNum, 127);
currentPage = startMotor + 10;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMotor + 10;
break;
}
}
break;
}
case(startAuton + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
autonMode--;
if(autonMode < 0)
autonMode = 1;
break;
}
case(LCD_BTN_RIGHT):
{
autonMode++;
if(autonMode > 1)
autonMode = 0;
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startAuton;
break;
}
}
break;
}
case(startMode + 1):
{
if (input == LCD_BTN_LEFT || input == LCD_BTN_RIGHT) {
if (opMode == 1) {
imeReset(IME_RIGHT_CHAIN_NUMBER);
imeReset(IME_LEFT_CHAIN_NUMBER);
}
else if (opMode == 2) {
setShooter(0);
}
}
switch(input)
{
case(LCD_BTN_LEFT):
{
opMode--;
if (opMode == -1) {
opMode = 2;
}
break;
}
case(LCD_BTN_RIGHT):
{
opMode++;
if (opMode == 3) {
opMode = 0;
}
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startMode;
break;
}
}
break;
}
case(startFPS + 1):
{
switch(input)
{
case(LCD_BTN_LEFT):
{
break;
}
case(LCD_BTN_RIGHT):
{
break;
}
case(LCD_BTN_CENTER):
{
currentPage = startFPS;
break;
}
}
break;
}
}
}
void motorBegin(int motor)
{
motorStop(motor);
}
void runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}
void stopLift(motor393 *motor1, motor393 *motor2){
motorStop(motor1);
motorStop(motor2);
}
// turnTo will turn a robot to the direction we give it here.
void Robot::turnTo(enum Direction newDirection)
{
//this speeds up either the left or right motor when turning so the robot lines up better orthogonally
//changes to 1 for 360 degree turn
int adjustMotors = 2;
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
// Get the relative direction to turn to based apon myDirection being north.
Direction relativeDirection = (enum Direction)modulo((newDirection - myDirection), 8);
// Then decide the number of ticks to turn.
int ticks = 0;
switch (relativeDirection)
{
case SOUTH:
ticks = 4*TURNANGLE360;//need the right ticks for 360 degrees
adjustMotors = 1;//both motors going the same speed works fine for this
break;
case SEAST:
ticks = 3*TURNANGLE;
break;
case EAST:
ticks = 2*TURNANGLE;
break;
case NEAST:
ticks = TURNANGLE;
break;
case NORTH:
break;
case SWEST:
ticks = -3*TURNANGLE;
break;
case WEST:
ticks = -2*TURNANGLE;
break;
case NWEST:
ticks = -TURNANGLE;
break;
}
// Start the motor.
// If it's positive, turn right.
if (ticks < 0)
{
MotorControl.write(0x88);//right forward
MotorControl.write(TURNSPEED*adjustMotors);
MotorControl.write(0x8E);//left back
MotorControl.write(TURNSPEED);
}
// If it's negative, turn left.
else if (ticks > 0)
{
MotorControl.write(0x8A);//right back
MotorControl.write(TURNSPEED);
MotorControl.write(0x8C);//left forward
MotorControl.write(TURNSPEED*adjustMotors);
}
// If it's equal, don't bother.
bool turnComplete = false;
int motorOne;
int motorTwo;
int motorOneSpeed = TURNSPEED;
int motorTwoSpeed = TURNSPEED;
// Turn till we've turned the ticks or until our gridSensor is on straight on line.
// We want to turn at least half the number of ticks before we check for the grid.
while (!turnComplete)
{
motorOne = abs(wheelEnc.getCountsM1());
motorTwo = abs(wheelEnc.getCountsM2());
// Here we are only using ticks in errorTickAdjustment to get a value as we only use ticks
int error = errorTickAdjustment();
// Calculate the speed that we should turn based on error.
motorOneSpeed = TURNSPEED + error;
motorTwoSpeed = TURNSPEED - error;
// Slow down by a bit towards the end.
if (motorOne > (abs(ticks) - 10) || motorTwo > (abs(ticks) - 10))
{
motorOneSpeed -= 20;
motorTwoSpeed -= 20;
}
/*
// If we've hit the number of ticks on either motor, or we've hit the
// grid line stop turning (after turning at least half the number of ticks).
unsigned int sensors[8];
gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if (((motorTwo > (ticks * 2 / 3)) || (motorOne > (ticks * 2 / 3)))
&& ((newDirection == NORTH) || (newDirection == SOUTH)
|| (newDirection == EAST) || (newDirection == WEST))
&& ((sensors[3] < SENSORBLACK) || (sensors[4] < SENSORBLACK)
|| (sensors[2] < SENSORBLACK) || (sensors[5] < SENSORBLACK)))
{
motorStop();
turnComplete = true;
}
else if (motorTwo > abs(ticks) || motorOne > abs(ticks))
*/
if (motorTwo > abs(ticks) || motorOne > abs(ticks))
{
motorStop();
turnComplete = true;
}
// Otherwise, continue turning based on the calculated error speed.
else
{
if (ticks < 0)
{
MotorControl.write(0x88);
MotorControl.write(limit(motorOneSpeed*adjustMotors));
MotorControl.write(0x8E);
MotorControl.write(limit(motorTwoSpeed));
}
else
{
MotorControl.write(0x8A);
MotorControl.write(limit(motorOneSpeed));
MotorControl.write(0x8C);
MotorControl.write(limit(motorTwoSpeed*adjustMotors));
}
}
delay(1);
}
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
// We are now facing 'newDirection'
myDirection = newDirection;
delay(100);
}
void operatorControl() {
int current_pot_val=analogRead(arm_pot);
if (digitalRead(2)==LOW) { //JUMPER IN DIG2=GO AUTONOMOUs
autonomous();
digitalWrite(limit_bot, HIGH);
digitalWrite(limit_top, HIGH);
}
while (1) {
//Drive motors, tank config
motorSet(DRIVE_FL, joystickGetAnalog(1,3));
motorSet(DRIVE_ML, joystickGetAnalog(1,3));
motorSet(DRIVE_FR, -joystickGetAnalog(1,2));
motorSet(DRIVE_MR, -joystickGetAnalog(1,2));
//Arm motors, right trigger buttons
if((joystickGetDigital(1,6,JOY_UP) == 1) && (digitalRead(limit_top) == 1)) {
current_pot_val=analogRead(arm_pot);
motorSet(ARM_TL, -127);
motorSet(ARM_BL, -127);
motorSet(ARM_TR, -127);
motorSet(ARM_BR, 127);
if(digitalRead(limit_top)==0) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
else if((joystickGetDigital(1,6,JOY_DOWN) == 1) && (digitalRead(limit_bot) == 1)) {
current_pot_val=analogRead(arm_pot);
motorSet(ARM_TL, 127);
motorSet(ARM_BL, 127);
motorSet(ARM_TR, 127);
motorSet(ARM_BR, -127);
if(digitalRead(limit_bot)==0) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
else {//keep arm up
if(analogRead(arm_pot)<=current_pot_val) {
//higher than idle
motorSet(ARM_TL, arm_idle_speed);
motorSet(ARM_BL, arm_idle_speed);
motorSet(ARM_TR, arm_idle_speed);
motorSet(ARM_BR, -(arm_idle_speed));
if((analogRead(arm_pot)==current_pot_val)||(digitalRead(limit_bot)==0)) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
if(analogRead(arm_pot)>=current_pot_val) {
//lower than idle
motorSet(ARM_TL, -(arm_idle_speed));
motorSet(ARM_BL, -(arm_idle_speed));
motorSet(ARM_TR, -(arm_idle_speed));
motorSet(ARM_BR, (arm_idle_speed));
if((analogRead(arm_pot)==current_pot_val)||(digitalRead(limit_top)==0)) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
}
//Intake motors, left trigger buttons
if(joystickGetDigital(1,5,JOY_UP) == 1) {
motorSet(IN_L, 127);
motorSet(IN_R, -127);
}
else if(joystickGetDigital(1,5,JOY_DOWN) == 1) {
motorSet(IN_L, -127);
motorSet(IN_R, 127);
}
else {
motorStop(IN_L);
motorStop(IN_R);
}
//preset arm heights
if(joystickGetDigital(1, 7,JOY_UP) == 1) {
while((analogRead(arm_pot)>2202) && (digitalRead(limit_top) == 1)) {
current_pot_val=2202;
motorSet(ARM_TL, -127);
motorSet(ARM_BL, -127);
motorSet(ARM_TR, -127);
motorSet(ARM_BR, 127);
if(digitalRead(limit_top)==0) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
}
if(joystickGetDigital(1, 7,JOY_DOWN) == 1) {
while((analogRead(arm_pot)<4040) && (digitalRead(limit_bot) == 1)) {
current_pot_val=4040;
motorSet(ARM_TL, 127);
motorSet(ARM_BL, 127);
motorSet(ARM_TR, 127);
motorSet(ARM_BR, -127);
if(digitalRead(limit_bot)==0) {
motorStop(ARM_TL);
motorStop(ARM_BL);
motorStop(ARM_TR);
motorStop(ARM_BR);
}
}
}
//LIGHTS
if(digitalRead(limit_top)==0)
digitalWrite(led_r, LOW);
else
digitalWrite(led_r, HIGH);
if(digitalRead(limit_bot)==0)
digitalWrite(led_g, LOW);
else
digitalWrite(led_g, HIGH);
}
printf("%d\r\n", current_pot_val);
}
void Move::stop() {
changeMoveState(MOV_STOP);
motorStop(MOTOR_LEFT);
motorStop(MOTOR_RIGHT);
}
void turnback(motor393 *motor_){
motorBack(&motor_[motorRight]);
motorStop(&motor_[motorLeft]);
delay(300);
}