void setWheelMotors(float leftSpeed, float rightSpeed) { if(!wheelsInitialized) { initWheels(); } SetMotor(motors[LEFT_WHEEL_MOTOR], leftSpeed); SetMotor(motors[RIGHT_WHEEL_MOTOR], rightSpeed); }
//Set motors with left and right variables void setm(float left, float right){ left=-left; SetMotor(Motors[0], left); SetMotor(Motors[1], left); SetMotor(Motors[2], right); SetMotor(Motors[3], right); }
//Stop all motors void stopMotors(void){ float left = 0, right = 0; SetMotor(Motors[0], left); SetMotor(Motors[1], left); SetMotor(Motors[2], right); SetMotor(Motors[3], right); }
void findEnd(void){ static int endFound = 0; static int bitchin = 0; SetPin(PIN_F1, 0); SetPin(PIN_F3, 1); if (wasLeftWall) { SetMotor(leftMotor, .5); SetMotor(rightMotor, .2); } else { SetMotor(leftMotor, .2); SetMotor(rightMotor, .5); } if (!linePresent()||mostDark()){ endFound+=1; }else{ endFound = 0; bitchin = 0; ///new addition } if(endFound == 50){ bitchin+=1; endFound = 0; } if (bitchin==6){ stage = 3; } SetPin(PIN_F1, 1); SetPin(PIN_F3, 0); }
void DriveSystemArcadeDrive( short forward, short turn ) { long drive_l_motor; long drive_r_motor; // Set drive drive_l_motor = forward + turn; drive_r_motor = forward - turn; // normalize drive so max is 127 if any drive is over 127 int max = abs(drive_l_motor); if (abs(drive_r_motor) > max) max = abs(drive_r_motor); if (max>127) { drive_l_motor = 127 * drive_l_motor / max; drive_r_motor = 127 * drive_r_motor / max; } // Send to motors // left drive SetMotor( MotorDriveL, drive_l_motor); // right drive SetMotor( MotorDriveR, drive_r_motor); }
void test() { SetMotor(2, 127); SetMotor(3, -127); SetMotor(6, 127); SetMotor(7, -127); Wait(1000); }
//Turns on all motors equallt void runMotor(void){ float left = 0, right = 0, speed = 1.0f, accel = .01f; left = -speed; right = speed; SetMotor(Motors[0], left); SetMotor(Motors[1], left); SetMotor(Motors[2], right); SetMotor(Motors[3], right); }
//TODO: Old code requires TargetRPM always be positive... here void setTargetDuty(char dc, unsigned char mot) { if(PID_ON) TargetRPM[mot]=MAX_RPM*dc/100; else { if(mot==LEFT) SetMotor(dc, mot); else SetMotor(dc, mot); } }
int MotorCompliance(void) { int StopSwitch = 0; int ForLoop = 1; int ArmEmergencyRelease = 0; unsigned int LetsGo = 0; /* Bring the motor down at first powerup so the robot is compliant. */ while(ForLoop) // This is the same while loop as in the main program, { // but is using a variable set that starts set to 1 /* Get digital input from the switch mounted to the tower. * GetDigitalInput is weird - 1 is unpressed, 0 is pressed */ StopSwitch = GetDigitalInput(7); if(StopSwitch == 1) { /* Button on the joypad to press in case the above DI doesn't trigger. * Same button as the arm unlock button. */ ArmEmergencyRelease = GetJoystickDigital(1,5,2); if(ArmEmergencyRelease == 1) { SetServo(6,0); ForLoop = 0; } /* Set various motors and servos to a compliant position */ SetMotor(2,-90); SetServo(4,64); SetServo(3,127); SetServo(6,127); SetServo(7,127); } else { LetsGo = GetJoystickAnalog(1,3); if(LetsGo) //Move Raisy in any position where it won't be set to 0 { SetServo(6,0); //Set Tilty to the straight position ForLoop = 0; //Exit the while loop } else { SetMotor(2,-40); // Keep Raisy down to the base until the joystick is moved } } } /* Return 1 when this function is called. We need this set to 1 so this init function only runs * once. MotorBringDown gets set to 1 and thus fails the if check when we do this. */ return 1; }
void SwingControl(short IPEncoder, float Time, short& State) { if(Time < 0.5) { SetMotor(152); State = 0; } else if(IPEncoder < 180) { SetMotor(0); State = 0; } else { SetMotor(0); State = 1; } }
void turn90Degrees(int dir){ if (dir == LEFT) { SetMotor(leftMotor, -1); SetMotor(rightMotor, 1); SysTick_Wait10ms(72); } if (dir == RIGHT){ SetMotor(leftMotor, 1); SetMotor(rightMotor, -1); SysTick_Wait10ms(72); } stage = 1; }
Drive_Arc(enum turnDir dir, unsigned int speed) { switch (dir) { case right: SetMotor(A, FORWARD, speed); SetMotor(B, REVERSE, speed); break; case left: SetMotor(A, REVERSE, speed); SetMotor(B, FORWARD, 0); break; default: break; } }
int main(void) { X1RCU_Init(); int traM(int D,int S,int A) { mtrP[0] = mtrP[3] = vd[D][0]*S/100; mtrP[1] = mtrP[2] = vd[D][1]*S/100; cmpr = (cmpr - A)%360; if (cmpr > 180) { rotS = (360 - cmpr)*5/9; rotD = 0; } else { rotS = cmpr*5/9; rotD = 1; } mtrC = S + rotS; if (rotD == 0) { mtrP[0] -= rotS; mtrP[1] += rotS; mtrP[2] -= rotS; mtrP[3] += rotS; } else if (rotD == 1) { mtrP[0] += rotS; mtrP[1] -= rotS; mtrP[2] += rotS; mtrP[3] -= rotS; } for (int i = 0;i < 4;i++) { if (mtrP[i] < 0) { mtrP[i] = -mtrP[i]; mtrS[i] = 2; } else { mtrS[i] = 0; } if (mtrP[i] > 100) { mtrP[i] *= 100/mtrC; } //SetLCD3Char((i*4+1),mtrP[i]); } SetMotor( _MOTOR_NE_,mtrS[0],mtrP[0]); SetMotor( _MOTOR_NW_,mtrS[1],mtrP[1]); SetMotor( _MOTOR_SE_,mtrS[2],mtrP[2]); SetMotor( _MOTOR_SW_,mtrS[3],mtrP[3]); return; }
/** * set power of all the motors in the group * @param group the initialized motor group * @param power the amount of power to give (from 127 to -127) **/ void setMotors(LV_MOTOR_GROUP *group, int power) { int i; for (i = 0; i <= group->_last && i < group->length; i += 1) { SetMotor(group->motors[i], group->inverse[i] * power); } }
task ArmPidController(void *arg) { int armSensorCurrentValue; int armError; float armDrive; static float pid_K = 0.3; (void)arg; vexTaskRegister("arm pid"); while( TRUE ) { // Read the sensor value and scale armSensorCurrentValue = vexAdcGet( armPot ); // calculate error armError = armRequestedValue - armSensorCurrentValue; // calculate drive armDrive = (pid_K * (float)armError); // limit drive if( armDrive > 127 ) armDrive = 127; else if( armDrive < (-127) ) armDrive = (-127); // send to motor SetMotor( MotorArm, armDrive); // Don't hog cpu wait1Msec( 25 ); } }
/** * power a group of specific motors inside of a larger group * @param motors an int-array of motor indexes * @param size the size of the int-array * @param group the initialized motor group * @param power the amount of power (between -127 to 127) **/ void setSMotors(int motors[], int size, LV_MOTOR_GROUP *group, int power) { int i, j; for (i = 0; i < size; i += 1) { j = motors[i]; SetMotor(group->motors[j], power); } }
inline void pidRoutine() { static unsigned char i; static char DC; static int delta=0; numSkips++; //only want this to execute every 2 times because we are set up to call it twice as fast as we want if(numSkips<2) return; else numSkips=0; //on 5th one we continue toggleTestPin(); for(i=0;i<NUM_PID;i++) { currTicks[i] = getAndResetNumTicks(i); //resets numTicks too } //NOTE: should also reset timer2 flag to allow this to get called again. //TIM1_TC6+= PERIOD; //TIM1_TFLG1=_S12_C6F; sei(); //enable global interrupt flag for(i=0;i<=NUM_PID;i++) { //if getDir is 1 (negative direction), negate RPM //TODO: CHECK MATH RPM[i]=currTicks[i]*.0165; //.0165 //1tick/50.4ms * 1000ms/sec*60sec/min*rev/512*1/141 gearbox RPMError[i]=TargetRPM[i] - RPM[i]; SumError[i]+=RPMError[i]; RequestedDuty[i]=((RPMError[i] * pGain)+(iGain*SumError[i])); if(RequestedDuty[i] > 100) { RequestedDuty[i] = 100; SumError[i]-=RPMError[i]; //anti Windup } else if(RequestedDuty[i] <-100) { RequestedDuty[i]=-100; SumError[i]-=RPMError[i]; //anti Windup } if(RPMError[i] > STALL_ERR_THRES) stallCt[i]++; else stallCt[i]=0; LastError[i]=RPMError[i]; //update DC=(char) RequestedDuty[i]; //--------SET MOTOR SPEEDS---------------- if (PID_ON) { SetMotor(DC,i); } } }
/******************************************************************************* * 函 数 名 :main * 函数功能 :主函数 * 输 入 :无 * 输 出 :无 *******************************************************************************/ void main() { Timer0Init(); InitMotor(); while(1) { SetMotor(); } }
void squareDance(void){ int turns = 0; bool postTurn = true; bool clockWise; float sensor; rightSensor = ADCRead(adc[1])*1000; leftSensor = ADCRead(adc[2])*1000; if (rightSensor>leftSensor){ clockWise = true; }else{ clockWise = false; } while(turns < 4) { if (clockWise) {sensor = ADCRead(adc[1])*1000;}else{sensor = ADCRead(adc[2])*1000;} if (postTurn){ //////after we make a turn SetMotor(leftMotor, 1); SetMotor(rightMotor, 1); SysTick_Wait10ms(250); // go straight for a short time to get back by object do { if (clockWise) {sensor = ADCRead(adc[1])*1000;}else{sensor = ADCRead(adc[2])*1000;} }while(sensor > 400); postTurn = false; } else { SetMotor(leftMotor, 1); SetMotor(rightMotor, 1); /////keep going straight while no wall is close do { if (clockWise) {sensor = ADCRead(adc[1])*1000;}else{sensor = ADCRead(adc[2])*1000;} }while(sensor < 400); /////now that we have encountered a wall /////keep going straight until we have passed the wall so we can make turn do { if (clockWise) {sensor = ADCRead(adc[1])*1000;}else{sensor = ADCRead(adc[2])*1000;} }while(sensor > 400); SysTick_Wait10ms(70); /////stop briefly SetMotor(leftMotor, 0); SetMotor(rightMotor, 0); SysTick_Wait10ms(40); /////turn corner if (clockWise) {turn90Degrees(RIGHT);} else { turn90Degrees(LEFT);} turns += 1; postTurn = true; } } //////once we have made 4 turns we will stop SetMotor(leftMotor, 0); SetMotor(rightMotor, 0); while(true); }
void motorDemo(void) { Printf("Press:\n w-forward\n s-backward\n a-left\n "); Printf("d-right\n space-stop\n enter-quit\n"); { float left = 0, right = 0, speed = 0.75; char newline = 13; char ch = Getc(); while(ch != newline) { ch = Getc(); Printf("%c", ch); switch(ch) { case 'w': left = speed; right = speed; break; case 's': left = -speed; right = -speed; break; case 'a': left = -speed; right = speed; break; case 'd': left = speed; right = -speed; break; default: left = 0; right = 0; break; } SetMotor(motors[0], left); SetMotor(motors[1], right); Printf(" Set Motor to %d %d \r", (int)(left*100), (int)(right*100)); } } SetMotor(motors[0], 0.0f); SetMotor(motors[1], 0.0f); Printf("\n"); }
int main(){ InitHardware(); int adc_reading = ReadAnalog(0); printf("%d\n",adc_reading); while(1){ adc_reading = ReadAnalog(0); if (adc_reading > 50){ SetMotor(1,1,255); SetMotor(2,1,255); Sleep(1,500000); } else{ SetMotor(1,1,0); SetMotor(2,1,0); } } return; }
void findLine(void){ static int lineFound = 0; if (wasLeftWall) { SetMotor(leftMotor, 1); SetMotor(rightMotor, .2); } else { SetMotor(leftMotor, .2); SetMotor(rightMotor, 1); } if (linePresent()){ lineFound+=1; }else{ lineFound = 0; } if(lineFound == 40){ //were at 40 stage = 2; } }
task ClawController(void *arg) { int hold_delay = 0; (void)arg; vexTaskRegister("claw control"); while( TRUE ) { switch( clawCmd ) { case kClawOpenCommand: // look for fully open (around 500 counts) if( vexMotorPositionGet( MotorClaw ) > 400 ) SetMotor( MotorClaw , CLAW_OPEN_LOW_SPEED); else SetMotor( MotorClaw , CLAW_OPEN_HIGH_SPEED); break; case kClawCloseCommand: // look for fully closed (enc == 0) if( vexMotorPositionGet( MotorClaw ) < 100 ) SetMotor( MotorClaw , CLAW_CLOSE_LOW_SPEED); else { // This test checks for slow motor movement when partially closed // half second delay before check starts if(hold_delay++ > 20) { if( SmartMotorGetSpeed( MotorClaw ) > -40 ) SetMotor( MotorClaw , CLAW_CLOSE_LOW_SPEED); } else SetMotor( MotorClaw , CLAW_CLOSE_HIGH_SPEED ); } // learn minimum encoder position if( vexMotorPositionGet( MotorClaw ) < 0 ) vexMotorPositionSet( MotorClaw, 0); break; case kClawStopCommand: SetMotor( MotorClaw , 0 ); clawCmd = kClawNoCommand; break; default: break; } // reset hold delay if(clawCmd != kClawCloseCommand) hold_delay = 0; // Don't hog cpu wait1Msec(25); } }
// Culture died in the 70's // with Forrest Dump // this method will take in two values from the IRSensors // and the line sensor and decide what to do with the // tires from there void runforrestrun(int ir, int line) { // Cases: // Line and no wall // Wall and no line // No line and no wall // IRead returns a distance // Depending on the distance, the motors // will curve a hard-coded amount of time (2.5 seconds) while(1) { SetMotor(Motors[0],0.2f); SetMotor(Motors[1],0.22f); Wait(12.0); SetMotor(Motors[0], 0); SetMotor(Motors[1], 0); Wait(5.0); SetMotor(Motors[0], -0.2f); SetMotor(Motors[1], -0.215f); Wait(10.0); } }
/** * write a relative power value to a motor * @param motor the pin of the motor * @param rpower the relative power (from 0 to 100, or from 0 to 1) * @param inverse whether to invert the power (1=Yes,0=No) * @return integer the absolute power used **/ int setRMotor(int motor, int rpower, int inverse) { // from percent to decimal if (rpower > 1) rpower /= 100; // from relative to absolute int power = rpower * 127 * (inverse == 1 ? -1 : 1); // write to pin SetMotor(motor, power); // return absolute value return power; }
void PIDControl(short IPEncoder, short MotorEncoder, float TimeStep) { static short init = 0; //static short IPEncoderRef = ENCODER_RANGE / 2, MotorEncoderRef = 0; static short IPEncoderRef = 0, MotorEncoderRef = 0; static float ScaleT = 0.85, KPT = 2.2, KIT = 3.6, KDT = 0.0055, \ ScaleX = 0.8, KPX = 0.25, KIX = 0.045, KDX = 0.02; static float IPEncoderErrorLast, IPEncoderErrorDiff, IPEncoderErrorInt = 0; static float MotorEncoderErrorLast, MotorEncoderErrorDiff, MotorEncoderErrorInt = 0; static float IPEncoderError, MotorEncoderError; static float ControlT, ControlX, ControlValue; if(init == 0) { MotorEncoderRef = MotorEncoder; init = 1; } IPEncoderError = IPEncoderRef - IPEncoder; if((IPEncoderError < 3) && (-3 < IPEncoderError)) IPEncoderError = 0; IPEncoderErrorDiff = (IPEncoderError - IPEncoderErrorLast) / TimeStep; IPEncoderErrorInt += IPEncoderError * TimeStep; IPEncoderErrorLast = IPEncoderError; MotorEncoderError = MotorEncoderRef - MotorEncoder; MotorEncoderErrorDiff = (MotorEncoderError - MotorEncoderErrorLast) / TimeStep; MotorEncoderErrorInt += MotorEncoderError * TimeStep; MotorEncoderErrorLast = MotorEncoderError; ControlT = KPT * IPEncoderError + KIT * IPEncoderErrorInt + KDT * IPEncoderErrorDiff; ControlX = KPX * MotorEncoderError + KIX * MotorEncoderErrorInt + KDX * MotorEncoderErrorDiff; ControlValue = ScaleT * ControlT + ScaleX * ControlX; //printf("PID\n"); //printf("IP: %d %f %f %f\n", IPEncoder, IPEncoderError, IPEncoderErrorDiff, IPEncoderErrorInt); //printf("Motor: %d %f %f %f\n", MotorEncoder, MotorEncoderError, MotorEncoderErrorDiff, MotorEncoderErrorInt); SetMotor(-short(ControlValue)); }
void parseSerial(uint8 *data,uint8 datasz) { data[datasz]='\0'; //Make sure it's null terminated. Arnold would approve. char *itr; char *name= strtok_r((char*)data,"-",&itr); //Motor is anything up to the first dash. char *val= strtok_r(NULL,"\0",&itr); //Speed is anything after the first dash. int value = atoi(val); int motor= atoi(name); char buff[255]; if(motor==0&&value==0)//SHUT. DOWN. EVERYTHING.. { PCComms_UartPutString("YOU IDIOT! YOU'LL KILL SOMEONE!\r\n"); ReallyDisableEverything(); return; } if(motor==10) //THE CLAW! { if(value==1) //OPEN { PCComms_UartPutString("OPENING CLAW\r\n"); CloseExhaust_Write(0); ClosePressure_Write(0); OpenExhaust_Write(1); OpenExhaust_Write(1); } else if(value==-1) //CLOSE { PCComms_UartPutString("CLOSING CLAW\r\n"); CloseExhaust_Write(1); ClosePressure_Write(1); OpenExhaust_Write(0); OpenExhaust_Write(0); } return; } if(motor ==11 || motor ==12 || motor ==13)//Red or Green light { if(motor==11) { PCComms_UartPutString("RED LIGHT\r\n"); Red_Write(value); } else if(motor==12) { PCComms_UartPutString("GREEN LIGHT\r\n"); Green_Write(value); } else { PCComms_UartPutString("AMBER LIGHT\r\n"); Pause_Register_Write(value); } return; } if(value==255) { snprintf(buff,255,"Motor %d is set to speed %d\r\n",motor,motors[motor]); PCComms_UartPutString(buff); } else if(value<64&&value>-64) //If it's an acceptable value { motors[motor]=value; SetMotor(motor,ScaleVal(value)); snprintf(buff,255,"Setting motor %d to speed %d\r\n",motor,value); PCComms_UartPutString(buff); } else { snprintf(buff,255,"Bad data. Got: motor:%d value:%d\r\n",motor,value); PCComms_UartPutString(buff); } }
void BipedDef::DisableMotor() { SetMotor(false); }
void BipedDef::EnableMotor() { SetMotor(true); }
char Drive_Stop(void) { SetMotor(A, FORWARD, 0); SetMotor(B, FORWARD, 0); return SUCCESS; }