static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) { int16_t speedL, speedR; if(speedVal < 0) { directionVal *= -1; } //if(directionVal > MAX/2) speedR = 3 * speedVal /2 - 7 * directionVal / 10; speedL = 3 * speedVal /2 + 7 * directionVal / 10; DRV_SetSpeed(speedL, speedR); }
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) { #if PL_CONFIG_HAS_SHELL uint8_t buf[48]; #endif uint8_t val; int16_t x, y, z; (void)size; (void)packet; switch(type) { #if PL_CONFIG_HAS_MOTOR case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */ { int8_t direction, speed; int16_t x1000, y1000; *handled = TRUE; direction = *data; /* get x data value */ speed = *(data+1); /* get y data value */ if (REMOTE_isVerbose) { UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: "); UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction); UTIL1_chcat(buf, sizeof(buf), ','); UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n"); SHELL_SendString(buf); } #if 0 /* using shell command */ UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty "); UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x)); SHELL_ParseCmd(buf); UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty "); UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y)); SHELL_ParseCmd(buf); #endif //x1000 = scaleJoystickTo1K(x); //y1000 = scaleJoystickTo1K(y); if (REMOTE_useJoystick) { REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */ } } break; #endif case RAPP_MSG_TYPE_JOYSTICK_BTN: *handled = TRUE; val = *data; /* get data value */ #if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE if (val=='F') { /* F button, disable remote */ SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500"); REMOTE_SetOnOff(FALSE); DRV_SetSpeed(0,0); /* turn off motors */ SHELL_SendString("Remote OFF\r\n"); } else if (val=='G') { /* center joystick button: enable remote */ SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000"); REMOTE_SetOnOff(TRUE); DRV_SetMode(DRV_MODE_SPEED); SHELL_SendString("Remote ON\r\n"); } else if (val=='C') { /* red 'C' button */ /*! \todo add functionality */ } else if (val=='A') { /* green 'A' button */ /*! \todo add functionality */ } #else *handled = FALSE; /* no shell and no buzzer? */ #endif break; default: break; } /* switch */ return ERR_OK; }
static void REMOTE_HandleMotorMsg(int16_t direction, int16_t speedMode, int16_t z) { #define SCALE_DOWN 30 #define MIN_VALUE 250 /* values below this value are ignored */ #define DRIVE_DOWN 1 #define SCALE_FROM_PERCENT (16383/100) #define NORMAL_SPEED (20 * SCALE_FROM_PERCENT) #define FAST_SPEED (80 * SCALE_FROM_PERCENT) #define FAST_TURN (30 * SCALE_FROM_PERCENT) if (!REMOTE_isOn) { LED1_Off(); return; } LED1_On(); if(speedMode == 4) { // left handed LED2_On(); MAZE_SetTurnHandleLeft(TRUE); LF_StartFollowing(); } if(speedMode == 5) { // right handed LED2_On(); MAZE_SetTurnHandleLeft(FALSE); LF_StartFollowing(); } switch(direction) { case 0: // straight if(speedMode == 0){ DRV_SetSpeed(0, 0); } else if (speedMode == 1) { DRV_SetSpeed(NORMAL_SPEED, NORMAL_SPEED); } else if (speedMode == 2) { DRV_SetSpeed(FAST_SPEED,FAST_SPEED ); } else if (speedMode == 3){ DRV_SetSpeed( -NORMAL_SPEED, -NORMAL_SPEED ); } break; case 1: // right if(speedMode == 0){ DRV_SetSpeed(NORMAL_SPEED, 0); } else if (speedMode == 1) { DRV_SetSpeed(FAST_SPEED, NORMAL_SPEED); } else if (speedMode == 2) { DRV_SetSpeed(FAST_SPEED,FAST_TURN ); } else if (speedMode == 3){ DRV_SetSpeed( -FAST_SPEED, -NORMAL_SPEED ); } break; case 2: // left if(speedMode == 0){ DRV_SetSpeed(0, NORMAL_SPEED); } else if (speedMode == 1) { DRV_SetSpeed(NORMAL_SPEED, FAST_SPEED); } else if (speedMode == 2) { DRV_SetSpeed(FAST_TURN,FAST_SPEED ); } else if (speedMode == 3){ DRV_SetSpeed( -NORMAL_SPEED, -FAST_SPEED ); } break; } /* if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) { int16_t speed, speedL, speedR; speed = speedVal/SCALE_DOWN; if (directionVal<0) { if (speed<0) { speedR = speed+(directionVal/SCALE_DOWN); } else { speedR = speed-(directionVal/SCALE_DOWN); } speedL = speed; } else { speedR = speed; if (speed<0) { speedL = speed-(directionVal/SCALE_DOWN); } else { speedL = speed+(directionVal/SCALE_DOWN); } } #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR); #endif } else if (speedVal>100 || speedVal<-100) { //* speed #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(speedVal, speedVal); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN); #endif } else if (directionVal>100 || directionVal<-100) { //* direction #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(directionVal/DRIVE_DOWN, -directionVal/DRIVE_DOWN); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN)); #endif } else { //* device flat on the table? #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } */ }
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) { #define MIN_VALUE 350 /* old: 250 - values below this value are ignored */ #define Nitro 2 #define Zuercher 4 /* gerade aus -> schnell */ if (!REMOTE_isOn) { return; } if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */ #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) { int16_t speed, speedL, speedR; speed = speedVal*2; if (directionVal<0) { if (speed<0) { speedR = speed+(directionVal); } else { speedR = speed-(directionVal); } speedL = speed; } else { speedR = speed; if (speed<0) { speedL = speed-(directionVal); } else { speedL = speed+(directionVal); } } #if PL_CONFIG_HAS_DRIVE if(NITRO){ DRV_SetSpeed(speedL*Nitro, speedR*Nitro); }else { DRV_SetSpeed(speedL, speedR); } #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR); #endif } else if (speedVal>100 || speedVal<-100) { /* speed */ #if PL_CONFIG_HAS_DRIVE if(NITRO){ DRV_SetSpeed(speedVal*3*Nitro, speedVal*3*Nitro); /* Zuercher & Nitro esch chli vell */ } else { DRV_SetSpeed(speedVal*Zuercher, speedVal*Zuercher); } #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN); #endif } else if (directionVal>110 || directionVal<-110) { /* direction */ #if PL_CONFIG_HAS_DRIVE if(NITRO){ DRV_SetSpeed(directionVal*Nitro, -directionVal*Nitro); } else { DRV_SetSpeed(directionVal, -directionVal); } #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN)); #endif } else { /* device flat on the table? */ #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } }
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) { #define SCALE_DOWN 30 #define MIN_VALUE 250 /* values below this value are ignored */ #define DRIVE_DOWN 1 if (!REMOTE_isOn) { return; } if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */ #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) { int16_t speed, speedL, speedR; speed = speedVal/SCALE_DOWN; if (directionVal<0) { if (speed<0) { speedR = speed+(directionVal/SCALE_DOWN); } else { speedR = speed-(directionVal/SCALE_DOWN); } speedL = speed; } else { speedR = speed; if (speed<0) { speedL = speed-(directionVal/SCALE_DOWN); } else { speedL = speed+(directionVal/SCALE_DOWN); } } #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR); #endif } else if (speedVal>100 || speedVal<-100) { /* speed */ #if PL_CONFIG_HAS_DRIVE if(nitro){ DRV_SetSpeed((nitrovalue+2)*speedVal, (nitrovalue+2)*speedVal); } else { DRV_SetSpeed(3*speedVal, 3*speedVal); } #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN); #endif } else if (directionVal>100 || directionVal<-100) { /* direction */ #if PL_CONFIG_HAS_DRIVE if(nitro){ DRV_SetSpeed((1+nitrovalue)*directionVal, -directionVal*(1+nitrovalue)); } else { DRV_SetSpeed(2*directionVal, -directionVal*2); } #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN)); #endif } else { /* device flat on the table? */ #if PL_CONFIG_HAS_DRIVE DRV_SetSpeed(0, 0); #else MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); #endif } }
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) { uint8_t res = ERR_OK; const unsigned char *p; int32_t val1, val2; if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) { DRV_PrintHelp(io); *handled = TRUE; } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) { DRV_PrintStatus(io); *handled = TRUE; } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed ", sizeof("drive speed ")-1)==0) { p = cmd+sizeof("drive speed"); if (UTIL1_xatoi(&p, &val1)==ERR_OK) { if (UTIL1_xatoi(&p, &val2)==ERR_OK) { if (DRV_SetSpeed(val1, val2)!=ERR_OK) { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } } else { CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) { Q4CLeft_SetPos(0); Q4CRight_SetPos(0); if (DRV_SetPos(0, 0)!=ERR_OK) { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } *handled = TRUE; } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) { p = cmd+sizeof("drive pos"); if (UTIL1_xatoi(&p, &val1)==ERR_OK) { if (UTIL1_xatoi(&p, &val2)==ERR_OK) { if (DRV_SetPos(val1, val2)!=ERR_OK) { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } *handled = TRUE; } else { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } } else { CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr); res = ERR_FAILED; } } else if (UTIL1_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) { p = cmd+sizeof("drive mode"); if (UTIL1_strcmp((char*)p, (char*)"none")==0) { if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) { res = ERR_FAILED; } } else if (UTIL1_strcmp((char*)p, (char*)"stop")==0) { if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) { res = ERR_FAILED; } } else if (UTIL1_strcmp((char*)p, (char*)"speed")==0) { if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) { res = ERR_FAILED; } } else if (UTIL1_strcmp((char*)p, (char*)"pos")==0) { if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) { res = ERR_FAILED; } } else { res = ERR_FAILED; } if (res!=ERR_OK) { CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr); } *handled = TRUE; } return res; }
static void StateMachine_Run(void){ int i; int distance; // Für Acceleration Sensor int16_t x = MMA1_GetXmg(); WAIT1_Waitms(5); int16_t y = MMA1_GetYmg(); switch (state) { case START: //SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n"); WAIT1_Waitms(4700); DRV_EnableDisable(1); DRV_SetSpeed(3000,3000); //WAIT1_Waitms(40); //DRV_SetSpeed(3000,3000); //state = ATTACK; //DRV_SetSpeed(-4200,4200); // MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD); //MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40); /*int x0; int x5; int x0_cal; int x5_cal; x0 = Get_Reflectance_Values(0); x0_cal = x0 / 15; x5 = Get_Reflectance_Values(5); x5_cal = x5 / 15; MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal); */ //uint16_t* calib_values_pointer; //calib_values_pointer = &SensorTimeType; //calib_values_pointer = S1_GetVal(); state = DRIVE; break; case DRIVE: DRV_SetSpeed(-4200,4200); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40); if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz { state = TURN; break; } distance = US_usToCentimeters(US_Measure_us(),22); if((distance <= 30) && (distance != 0)){ state = ATTACK; } /* if((x >= 2000) || (x <= 2000)) { state = STOP; } if((y >= 2000) || (y <= 2000)) { state = STOP; } */ break; case TURN: //SHELL_SendString((unsigned char*)"...stopping calibration.\r\n"); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40); // ATTACK DRV_SetSpeed(-5000,-5000); WAIT1_Waitms(500); /* MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40); */ DRV_SetSpeed(4200,-4200); WAIT1_Waitms(300); DRV_SetSpeed(5000,5000); WAIT1_Waitms(50); if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500)) { state = DRIVE; } break; case ATTACK: DRV_SetSpeed(5500,5500); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80); if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz { state = TURN; } /* if((x >= 2000) || (x <= -2000)) { state = STOP; } if((y>= 2000) || (y <= -2000)) { state = STOP; } */ break; case STOP: DRV_SetSpeed(0,0); /*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/ /* if((x >= -300) && (x <= 300)) { if((y >= -300) && (y <= 300)) { state = DRIVE; } } */ break; } /* whitch */ }