File content as of revision 1:58b20a731e15:

#include "mechanism.h"

Mechanism::Mechanism(Serial* serial)
    //////nano   (LEDTX,LEDRX,LEDBAUD)
    
//encoder(encoder1, encoder2, NC, PPR, QEI::X4_ENCODING)
{
    md = new ikarashiMDC*[12];
    for(int i = 0; i < 4; i++) md[i] = new ikarashiMDC(1,i,SM,serial);
    for(int i = 0; i < 4; i++) md[i]->braking = true;
    for(int i = 0; i < 4; i++) omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
    for(int i = 4; i < 8; i++)md[i] = new ikarashiMDC(2,i-4,SM,serial);
    for(int i = 8; i < 11; i++)md[i] = new ikarashiMDC(3,i-7,SM,serial);
    for(int i = 4; i < 12; i++)md[i]-> braking = true;

}

void Mechanism::sendHeader()
{
}

void Mechanism::getTrigger(int Trigger)
{
    trigger = Trigger;
}

void Mechanism::up()
{ 
    md[8]-> setSpeed(riseSpeed);
    //nano .putc(0x01);
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
//    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
     
}

void Mechanism::down()
{
    md[8]-> setSpeed(follSpeed);
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
//    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::collect()
{
    if(trigger >= 50){
        md[10]-> setSpeed(collectSpeed);
    }
    else
    {
        md[10]-> setSpeed(-collectSpeed);
    }
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
//    md[10]-> setSpeed(0);
}

void Mechanism::hookRight()
{
    if(trigger >= 50){
        md[4]-> setSpeed(-hookSpeed);
    }
    else
    {
        md[4]-> setSpeed(hookSpeed);
    }
//    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::hookLeft()
{
    if(trigger >= 50){
        md[6]-> setSpeed(hookSpeed);
    }
    else
    {
        md[6]-> setSpeed(-hookSpeed);
    }
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
//    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::loadRight()
{
    if(trigger >= 50){
        md[5]-> setSpeed(loadSpeed);
    }
    else{
        md[5]-> setSpeed(-loadSpeed);
    }
    md[4]-> setSpeed(0);
//    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);    
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::loadLeft()
{
    if(trigger >= 50){
        md[7]-> setSpeed(loadSpeed);
    }
    else{
        md[7]-> setSpeed(-loadSpeed);
    }
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
//    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::lift()
{
    if(trigger >= 50){
        md[9]-> setSpeed(liftSpeed);
    }
    else{
        md[9]-> setSpeed(-liftSpeed);
    }
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
//    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}

void Mechanism::setBrake(int trigger)
{
    brake = trigger / 255.0;
}

void Mechanism::move(float x,float y,float turnPower)
{
    omni.computeXY(x,y,turnPower);
    for (int i = 0; i < 4; ++i) wheel[i] = omni.wheel[i];
    for (int i = 0; i < 4; ++i) md[i]->setSpeed(wheel[i] * brake);
}

void Mechanism::stop()
{
    for (int i = 0; i < 4; ++i) md[i]->setSpeed(0);
}

void Mechanism::noAct()
{
    md[4]-> setSpeed(0);
    md[5]-> setSpeed(0);
    md[6]-> setSpeed(0);
    md[7]-> setSpeed(0);
    md[8]-> setSpeed(0);
    md[9]-> setSpeed(0);
    md[10]-> setSpeed(0);
}