void RedBot::rotate(float power) { if (power < 0) { //Positive angle (counterclockwise): motor0(-power); //Left motor1(power); //Right } else { //Negative angle (clockwise): motor0(power); //Left motor1(-power); //Right } }
////////////////////////// // Forward Proportional // // Wall follow // ////////////////////////// void forwardP() { //error = gWallCurrent - s5avg; //instant error e error = s4avg-s5avg; P = Kp*error; D = Kd*(error-lasterror); output=P+D; lasterror=error; routput = (int)((BASE)+(output)); loutput = (int)((BASE)-(output)); // saturation logic if(routput > MAX) // no motor values > 255 or < 0 routput = MAX; else if(routput <= MIN) routput = MIN; if(loutput > MAX) // no motor values > 255 or < 0 loutput = MAX; else if(loutput <= MIN) loutput = MIN; if (routput == MIN) { motor1(BASE); //Left motor PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 1; //Motor L2 } else { motor1(routput); //Left motor PTT_PTT3 = 0; //Motor L1 PTT_PTT5 = 1; //Motor L2 } if (loutput == MIN) { motor0(BASE); //Right motor PTT_PTT4 = 1; //Motor R1 PTT_PTT6 = 1; //Motor R2 } else { motor0(loutput); //Right motor PTT_PTT4 = 0; //Motor R1 PTT_PTT6 = 1; //Motor R2 } //pwm }
void backSplit(int rspeed, int lspeed){ motor0(rspeed); motor1(lspeed); PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 0; //Motor L2 PTT_PTT4 = 1; //Motor R1 PTT_PTT6 = 0; //Motor R2 }
void back(int speed){ motor0(speed); motor1(speed); PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 0; //Motor L2 PTT_PTT4 = 1; //Motor R1 PTT_PTT6 = 0; //Motor R2 }
void leftManual(int rspeed, int lspeed){ motor0(rspeed); motor1(lspeed); PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 0; //Motor L2 PTT_PTT4 = 0; //Motor R1 PTT_PTT6 = 1; //Motor R2 }
void forward(int speed) { motor0(speed); motor1(speed); PTT_PTT3 = 0; //Motor L1 PTT_PTT5 = 1; //Motor L2 PTT_PTT4 = 0; //Motor R1 PTT_PTT6 = 1; //Motor R2 }
void left(int rspeed){ motor0(rspeed); motor1(0xFF); PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 1; //Motor L2 PTT_PTT4 = 0; //Motor R1 PTT_PTT6 = 1; //Motor R2 }
////////////// // Movement // ////////////// void stop() { motor0(0xFF); motor1(0xFF); PTT_PTT3 = 1; //Motor L1 PTT_PTT5 = 1; //Motor L2 PTT_PTT4 = 1; //Motor R1 PTT_PTT6 = 1; //Motor R2 }
int main() { HBridgeST motor0(Prop0A, Prop0B, Prop0_PWM, Tim2, 4); int v(0); while(1) { v+=100; v%= 1024; motor0.setSpeed(v); time.msleep(500); } }
void stop(){ motor0(127); motor1(127); }
void move(u08 dir) { motor0(dir); motor1(dir); }
void dumpSuperBall() { servo(0, 255); motor0(-COLLECTOR_SPEED); delayMs(2000); motor0(127); }
void turnOffCollector() { motor1(127); motor0(127); }
void turnOnCollector() { motor1(SHOOTER_SPEED); delayMs(3000); motor0(COLLECTOR_SPEED); }
void RedBot::move(float power) { motor0(power); motor1(power); }
void set_rgb_led(char r, char g, char b){ motor0(r); motor1(g); motor2(b); }
void RedBot::move(float powerLeft, float powerRight) { motor0(powerLeft); motor1(powerRight); }