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