機構部
Dependents: 2019BSyudo Bsyudo 2019Bsyudo
mechanism.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-10-04
- Revision:
- 3:ff9bb9f8fabe
- Parent:
- 1:58b20a731e15
File content as of revision 3:ff9bb9f8fabe:
#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; omni.wheel[0].setRadian(PI / 4.0 * 1.0); omni.wheel[1].setRadian(PI / 4.0 * 3.0); omni.wheel[2].setRadian(PI / 4.0 * 5.0); omni.wheel[3].setRadian(PI / 4.0 * 7.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,float nowAngle, float norm) { rad = atan2(y,x) + (nowAngle * (PI / 180.0)); omni.computeXY(norm * cos(rad),norm * sin(rad),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); }