hirromu takaki / mechanism

機構部

Dependents:   2019BSyudo Bsyudo 2019Bsyudo

mechanism.cpp@3:ff9bb9f8fabe, 2019-10-04 (annotated)

Committer:
THtakahiro702286
Date:
Fri Oct 04 08:15:54 2019 +0000
Revision:
3:ff9bb9f8fabe
Parent:
1:58b20a731e15 
mechanism

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:0b7a52a20594 1 #include "mechanism.h"
THtakahiro702286 0:0b7a52a20594 2
THtakahiro702286 1:58b20a731e15 3 Mechanism::Mechanism(Serial* serial)
THtakahiro702286 1:58b20a731e15 4 //////nano (LEDTX,LEDRX,LEDBAUD)
THtakahiro702286 0:0b7a52a20594 5
THtakahiro702286 1:58b20a731e15 6 //encoder(encoder1, encoder2, NC, PPR, QEI::X4_ENCODING)
THtakahiro702286 0:0b7a52a20594 7 {
THtakahiro702286 1:58b20a731e15 8 md = new ikarashiMDC*[12];
THtakahiro702286 1:58b20a731e15 9 for(int i = 0; i < 4; i++) md[i] = new ikarashiMDC(1,i,SM,serial);
THtakahiro702286 1:58b20a731e15 10 for(int i = 0; i < 4; i++) md[i]->braking = true;
THtakahiro702286 3:ff9bb9f8fabe 11 omni.wheel[0].setRadian(PI / 4.0 * 1.0);
THtakahiro702286 3:ff9bb9f8fabe 12 omni.wheel[1].setRadian(PI / 4.0 * 3.0);
THtakahiro702286 3:ff9bb9f8fabe 13 omni.wheel[2].setRadian(PI / 4.0 * 5.0);
THtakahiro702286 3:ff9bb9f8fabe 14 omni.wheel[3].setRadian(PI / 4.0 * 7.0);
THtakahiro702286 1:58b20a731e15 15 for(int i = 4; i < 8; i++)md[i] = new ikarashiMDC(2,i-4,SM,serial);
THtakahiro702286 1:58b20a731e15 16 for(int i = 8; i < 11; i++)md[i] = new ikarashiMDC(3,i-7,SM,serial);
THtakahiro702286 1:58b20a731e15 17 for(int i = 4; i < 12; i++)md[i]-> braking = true;
THtakahiro702286 1:58b20a731e15 18
THtakahiro702286 0:0b7a52a20594 19 }
THtakahiro702286 1:58b20a731e15 20
THtakahiro702286 1:58b20a731e15 21 void Mechanism::sendHeader()
THtakahiro702286 1:58b20a731e15 22 {
THtakahiro702286 1:58b20a731e15 23 }
THtakahiro702286 1:58b20a731e15 24
THtakahiro702286 0:0b7a52a20594 25 void Mechanism::getTrigger(int Trigger)
THtakahiro702286 0:0b7a52a20594 26 {
THtakahiro702286 0:0b7a52a20594 27 trigger = Trigger;
THtakahiro702286 0:0b7a52a20594 28 }
THtakahiro702286 0:0b7a52a20594 29
THtakahiro702286 0:0b7a52a20594 30 void Mechanism::up()
THtakahiro702286 0:0b7a52a20594 31 {
THtakahiro702286 1:58b20a731e15 32 md[8]-> setSpeed(riseSpeed);
THtakahiro702286 1:58b20a731e15 33 //nano .putc(0x01);
THtakahiro702286 1:58b20a731e15 34 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 35 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 36 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 37 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 38 // md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 39 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 40 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 41
THtakahiro702286 0:0b7a52a20594 42 }
THtakahiro702286 0:0b7a52a20594 43
THtakahiro702286 0:0b7a52a20594 44 void Mechanism::down()
THtakahiro702286 0:0b7a52a20594 45 {
THtakahiro702286 1:58b20a731e15 46 md[8]-> setSpeed(follSpeed);
THtakahiro702286 1:58b20a731e15 47 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 48 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 49 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 50 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 51 // md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 52 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 53 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 54 }
THtakahiro702286 0:0b7a52a20594 55
THtakahiro702286 0:0b7a52a20594 56 void Mechanism::collect()
THtakahiro702286 0:0b7a52a20594 57 {
THtakahiro702286 1:58b20a731e15 58 if(trigger >= 50){
THtakahiro702286 1:58b20a731e15 59 md[10]-> setSpeed(collectSpeed);
THtakahiro702286 0:0b7a52a20594 60 }
THtakahiro702286 0:0b7a52a20594 61 else
THtakahiro702286 0:0b7a52a20594 62 {
THtakahiro702286 1:58b20a731e15 63 md[10]-> setSpeed(-collectSpeed);
THtakahiro702286 0:0b7a52a20594 64 }
THtakahiro702286 1:58b20a731e15 65 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 66 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 67 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 68 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 69 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 70 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 71 // md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 72 }
THtakahiro702286 0:0b7a52a20594 73
THtakahiro702286 0:0b7a52a20594 74 void Mechanism::hookRight()
THtakahiro702286 0:0b7a52a20594 75 {
THtakahiro702286 1:58b20a731e15 76 if(trigger >= 50){
THtakahiro702286 1:58b20a731e15 77 md[4]-> setSpeed(-hookSpeed);
THtakahiro702286 0:0b7a52a20594 78 }
THtakahiro702286 0:0b7a52a20594 79 else
THtakahiro702286 0:0b7a52a20594 80 {
THtakahiro702286 1:58b20a731e15 81 md[4]-> setSpeed(hookSpeed);
THtakahiro702286 0:0b7a52a20594 82 }
THtakahiro702286 1:58b20a731e15 83 // md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 84 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 85 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 86 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 87 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 88 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 89 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 90 }
THtakahiro702286 0:0b7a52a20594 91
THtakahiro702286 0:0b7a52a20594 92 void Mechanism::hookLeft()
THtakahiro702286 0:0b7a52a20594 93 {
THtakahiro702286 1:58b20a731e15 94 if(trigger >= 50){
THtakahiro702286 1:58b20a731e15 95 md[6]-> setSpeed(hookSpeed);
THtakahiro702286 0:0b7a52a20594 96 }
THtakahiro702286 0:0b7a52a20594 97 else
THtakahiro702286 0:0b7a52a20594 98 {
THtakahiro702286 1:58b20a731e15 99 md[6]-> setSpeed(-hookSpeed);
THtakahiro702286 0:0b7a52a20594 100 }
THtakahiro702286 1:58b20a731e15 101 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 102 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 103 // md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 104 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 105 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 106 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 107 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 108 }
THtakahiro702286 0:0b7a52a20594 109
THtakahiro702286 0:0b7a52a20594 110 void Mechanism::loadRight()
THtakahiro702286 0:0b7a52a20594 111 {
THtakahiro702286 1:58b20a731e15 112 if(trigger >= 50){
THtakahiro702286 1:58b20a731e15 113 md[5]-> setSpeed(loadSpeed);
THtakahiro702286 0:0b7a52a20594 114 }
THtakahiro702286 0:0b7a52a20594 115 else{
THtakahiro702286 1:58b20a731e15 116 md[5]-> setSpeed(-loadSpeed);
THtakahiro702286 0:0b7a52a20594 117 }
THtakahiro702286 1:58b20a731e15 118 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 119 // md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 120 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 121 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 122 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 123 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 124 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 125 }
THtakahiro702286 0:0b7a52a20594 126
THtakahiro702286 0:0b7a52a20594 127 void Mechanism::loadLeft()
THtakahiro702286 0:0b7a52a20594 128 {
THtakahiro702286 1:58b20a731e15 129 if(trigger >= 50){
THtakahiro702286 0:0b7a52a20594 130 md[7]-> setSpeed(loadSpeed);
THtakahiro702286 0:0b7a52a20594 131 }
THtakahiro702286 0:0b7a52a20594 132 else{
THtakahiro702286 0:0b7a52a20594 133 md[7]-> setSpeed(-loadSpeed);
THtakahiro702286 0:0b7a52a20594 134 }
THtakahiro702286 1:58b20a731e15 135 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 136 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 137 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 138 // md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 139 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 140 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 141 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 142 }
THtakahiro702286 0:0b7a52a20594 143
THtakahiro702286 0:0b7a52a20594 144 void Mechanism::lift()
THtakahiro702286 0:0b7a52a20594 145 {
THtakahiro702286 1:58b20a731e15 146 if(trigger >= 50){
THtakahiro702286 1:58b20a731e15 147 md[9]-> setSpeed(liftSpeed);
THtakahiro702286 0:0b7a52a20594 148 }
THtakahiro702286 0:0b7a52a20594 149 else{
THtakahiro702286 1:58b20a731e15 150 md[9]-> setSpeed(-liftSpeed);
THtakahiro702286 0:0b7a52a20594 151 }
THtakahiro702286 1:58b20a731e15 152 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 153 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 154 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 155 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 156 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 157 // md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 158 md[10]-> setSpeed(0);
THtakahiro702286 0:0b7a52a20594 159 }
THtakahiro702286 1:58b20a731e15 160
THtakahiro702286 1:58b20a731e15 161 void Mechanism::setBrake(int trigger)
THtakahiro702286 1:58b20a731e15 162 {
THtakahiro702286 1:58b20a731e15 163 brake = trigger / 255.0;
THtakahiro702286 1:58b20a731e15 164 }
THtakahiro702286 1:58b20a731e15 165
THtakahiro702286 3:ff9bb9f8fabe 166 void Mechanism::move(float x,float y,float turnPower,float nowAngle, float norm)
THtakahiro702286 1:58b20a731e15 167 {
THtakahiro702286 3:ff9bb9f8fabe 168 rad = atan2(y,x) + (nowAngle * (PI / 180.0));
THtakahiro702286 3:ff9bb9f8fabe 169 omni.computeXY(norm * cos(rad),norm * sin(rad),turnPower);
THtakahiro702286 1:58b20a731e15 170 for (int i = 0; i < 4; ++i) wheel[i] = omni.wheel[i];
THtakahiro702286 1:58b20a731e15 171 for (int i = 0; i < 4; ++i) md[i]->setSpeed(wheel[i] * brake);
THtakahiro702286 1:58b20a731e15 172 }
THtakahiro702286 1:58b20a731e15 173
THtakahiro702286 1:58b20a731e15 174 void Mechanism::stop()
THtakahiro702286 1:58b20a731e15 175 {
THtakahiro702286 1:58b20a731e15 176 for (int i = 0; i < 4; ++i) md[i]->setSpeed(0);
THtakahiro702286 1:58b20a731e15 177 }
THtakahiro702286 1:58b20a731e15 178
THtakahiro702286 1:58b20a731e15 179 void Mechanism::noAct()
THtakahiro702286 1:58b20a731e15 180 {
THtakahiro702286 1:58b20a731e15 181 md[4]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 182 md[5]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 183 md[6]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 184 md[7]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 185 md[8]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 186 md[9]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 187 md[10]-> setSpeed(0);
THtakahiro702286 1:58b20a731e15 188 }