main.cpp

Go to the documentation of this file.

#include "MyRIO.h"
#include <math.h>

using namespace myRIO;
using namespace std;

double map(double x, double in_min, double in_max, double out_min, double out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int main() {
    if(!myRIO_init()) {cout << "Error initializing myRIO"; return -1;}

    Motor motorLeft(PWMA1, CCW, 8.823);
    Motor motorRight(PWMA0, CW, 8.831);

    PWM buzzer(PWMA2, 300, 0);
    PWM ledForce(PWMB1, 10e3, 0);
    PWM ledProx(PWMB2, 10e3, 0);

    MotorPID motorLeftPid(0.95, 40);
    MotorPID motorRightPid(0.75, 35);

    const long sampleTimeUs = 500;

    Wifi w([&](long data) {
        double motorLeftSpeed = static_cast<char>((data&0xFF00)>>8);
        double motorRightSpeed = static_cast<char>(data&0xFF);

        if(motorLeftSpeed > 127) motorLeftSpeed -=  255;
        if(motorRightSpeed > 127) motorRightSpeed -= 255;

        motorLeftSpeed = map(motorLeftSpeed, -128, 127, -850, 850);
        motorRightSpeed = map(motorRightSpeed, -128, 127, -850, 850);

        if(abs((int)ceil(motorLeftSpeed))<50) motorLeftSpeed = 0;
        if(abs((int)ceil(motorRightSpeed))<50) motorRightSpeed = 0;


        motorLeftPid.setSetpoint(motorLeftSpeed);
        motorRightPid.setSetpoint(motorRightSpeed);
        
        DIO::writeLed(LED0, data&0x40000000);
        DIO::writeLed(LED1, data&0x20000000);
        DIO::writeLed(LED2, data&0x10000000);
        DIO::writeLed(LED3, data&0x08000000);
        
        if(data&0x08000000)
            buzzer.setDutyCycle(50);
        else
            buzzer.setDutyCycle(0);
    });
    //while(!w.isConnected());

    while(1) {
        long encL = motorLeft.getEncoderPulses();
        if(motorLeft.getDefaultDirection()==CCW) encL = - encL;
        double correctedL = motorLeftPid.compute(encL);
        motorLeft.setAngularSpeedAndDirection(correctedL);

        long encR = motorRight.getEncoderPulses();
        if(motorRight.getDefaultDirection()==CCW) encR = - encR;
        double correctedR = motorRightPid.compute(encR);
        motorRight.setAngularSpeedAndDirection(correctedR);

        Time::wait_us(sampleTimeUs);


        double force = AIO::readPin(AI0);
        double dutyCycle = map(force, 0, 5, 0, 100);
        if(dutyCycle>100) dutyCycle = 100;
        ledForce.setDutyCycle(dutyCycle);

        double proxi = AIO::readPin(BI2);
        proxi = 27.726*pow(proxi, -1.2045);
        dutyCycle = map(proxi, 10, 100, 100, 0);
        if(dutyCycle>100) dutyCycle = 100;
        if(dutyCycle<0) dutyCycle = 0;
        ledProx.setDutyCycle(dutyCycle);
    }

    return 0;
}