hirromu takaki / mechanism

機構部

Dependents:   2019BSyudo Bsyudo 2019Bsyudo

mechanism.cpp@1:58b20a731e15, 2019-09-13 (annotated)

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