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;
}
