機構部
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?
User | Revision | Line number | New 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 | } |