ケンタ ミヤザキ
kkkk
main.cpp@0:158455b60200, 2019-01-19 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Jan 19 12:13:21 2019 +0000
- Revision:
- 0:158455b60200
- Child:
- 1:681efc14eba9
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| kenken0721 | 0:158455b60200 | 1 | #include "mbed.h" |
| kenken0721 | 0:158455b60200 | 2 | #include "speedservo.h" |
| kenken0721 | 0:158455b60200 | 3 | |
| kenken0721 | 0:158455b60200 | 4 | //肘 |
| kenken0721 | 0:158455b60200 | 5 | Servo RFelbow(PA_13); |
| kenken0721 | 0:158455b60200 | 6 | Servo LFelbow(PA_15); |
| kenken0721 | 0:158455b60200 | 7 | Servo RBelbow(PC_2); |
| kenken0721 | 0:158455b60200 | 8 | Servo LBelbow(PC_13); |
| kenken0721 | 0:158455b60200 | 9 | |
| kenken0721 | 0:158455b60200 | 10 | //膝 |
| kenken0721 | 0:158455b60200 | 11 | Servo RFknee(PA_14); |
| kenken0721 | 0:158455b60200 | 12 | Servo LFknee(PC_1); |
| kenken0721 | 0:158455b60200 | 13 | Servo RBknee(PC_3); |
| kenken0721 | 0:158455b60200 | 14 | Servo LBknee(PC_7); |
| kenken0721 | 0:158455b60200 | 15 | |
| kenken0721 | 0:158455b60200 | 16 | //肩 |
| kenken0721 | 0:158455b60200 | 17 | Serial SerialServo(PC_4,PA_10); |
| kenken0721 | 0:158455b60200 | 18 | |
| kenken0721 | 0:158455b60200 | 19 | DigitalIn Dmode(PB_13);//歩行方向変更 |
| kenken0721 | 0:158455b60200 | 20 | DigitalIn start_button(PB_2); |
| kenken0721 | 0:158455b60200 | 21 | |
| kenken0721 | 0:158455b60200 | 22 | AnalogIn var(PC_0); |
| kenken0721 | 0:158455b60200 | 23 | Serial PC(USBTX,USBRX); |
| kenken0721 | 0:158455b60200 | 24 | |
| kenken0721 | 0:158455b60200 | 25 | int ser_spe[4] = {32,32,32,32}; |
| kenken0721 | 0:158455b60200 | 26 | int ser_pos[4] = {135,135,135,135}; |
| kenken0721 | 0:158455b60200 | 27 | int now_pos[4] = {}; |
| kenken0721 | 0:158455b60200 | 28 | |
| kenken0721 | 0:158455b60200 | 29 | int prePos = 2000; |
| kenken0721 | 0:158455b60200 | 30 | int behPos = 1700; |
| kenken0721 | 0:158455b60200 | 31 | int upPos = 850; |
| kenken0721 | 0:158455b60200 | 32 | int downPos = 1350; |
| kenken0721 | 0:158455b60200 | 33 | |
| kenken0721 | 0:158455b60200 | 34 | float fmap(float x, float in_min, float in_max, float out_min, float out_max){ |
| kenken0721 | 0:158455b60200 | 35 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
| kenken0721 | 0:158455b60200 | 36 | } |
| kenken0721 | 0:158455b60200 | 37 | |
| kenken0721 | 0:158455b60200 | 38 | |
| kenken0721 | 0:158455b60200 | 39 | void serServoMove(int num,int angle,int speed){ |
| kenken0721 | 0:158455b60200 | 40 | ser_spe[num] = speed; |
| kenken0721 | 0:158455b60200 | 41 | ser_pos[num] = angle; |
| kenken0721 | 0:158455b60200 | 42 | } |
| kenken0721 | 0:158455b60200 | 43 | |
| kenken0721 | 0:158455b60200 | 44 | void bluepill_rx(){ |
| kenken0721 | 0:158455b60200 | 45 | if(SerialServo.readable()){ |
| kenken0721 | 0:158455b60200 | 46 | int request = SerialServo.getc(); |
| kenken0721 | 0:158455b60200 | 47 | if(request >= 0 && request <= 3){ |
| kenken0721 | 0:158455b60200 | 48 | now_pos[request] = SerialServo.getc(); |
| kenken0721 | 0:158455b60200 | 49 | SerialServo.putc(request); |
| kenken0721 | 0:158455b60200 | 50 | SerialServo.putc(ser_spe[request]); |
| kenken0721 | 0:158455b60200 | 51 | SerialServo.putc(ser_pos[request]); |
| kenken0721 | 0:158455b60200 | 52 | } |
| kenken0721 | 0:158455b60200 | 53 | } |
| kenken0721 | 0:158455b60200 | 54 | } |
| kenken0721 | 0:158455b60200 | 55 | void centerMove(int dev,int num){ |
| kenken0721 | 0:158455b60200 | 56 | for(int i=0;i<4;i++){ |
| kenken0721 | 0:158455b60200 | 57 | serServoMove(i,135+dev,30); |
| kenken0721 | 0:158455b60200 | 58 | } |
| kenken0721 | 0:158455b60200 | 59 | /* |
| kenken0721 | 0:158455b60200 | 60 | if(i != num){ |
| kenken0721 | 0:158455b60200 | 61 | serServoMove(i,135+dev,30); |
| kenken0721 | 0:158455b60200 | 62 | } |
| kenken0721 | 0:158455b60200 | 63 | } |
| kenken0721 | 0:158455b60200 | 64 | */ |
| kenken0721 | 0:158455b60200 | 65 | |
| kenken0721 | 0:158455b60200 | 66 | } |
| kenken0721 | 0:158455b60200 | 67 | |
| kenken0721 | 0:158455b60200 | 68 | void RFmove(int position,int speed){ |
| kenken0721 | 0:158455b60200 | 69 | if(2350 <= position){ |
| kenken0721 | 0:158455b60200 | 70 | position = 2350; |
| kenken0721 | 0:158455b60200 | 71 | } |
| kenken0721 | 0:158455b60200 | 72 | RFelbow.SpeedConvert(speed); |
| kenken0721 | 0:158455b60200 | 73 | RFelbow.SetPosition(fmap(position,850,2350,2350,850)); |
| kenken0721 | 0:158455b60200 | 74 | } |
| kenken0721 | 0:158455b60200 | 75 | |
| kenken0721 | 0:158455b60200 | 76 | void LFmove(int position,int speed){ |
| kenken0721 | 0:158455b60200 | 77 | if(2350 <= position){ |
| kenken0721 | 0:158455b60200 | 78 | position = 2350; |
| kenken0721 | 0:158455b60200 | 79 | } |
| kenken0721 | 0:158455b60200 | 80 | |
| kenken0721 | 0:158455b60200 | 81 | LFelbow.SpeedConvert(speed); |
| kenken0721 | 0:158455b60200 | 82 | LFelbow.SetPosition(position); |
| kenken0721 | 0:158455b60200 | 83 | } |
| kenken0721 | 0:158455b60200 | 84 | |
| kenken0721 | 0:158455b60200 | 85 | void RBmove(int position,int speed){ |
| kenken0721 | 0:158455b60200 | 86 | if(2350 <= position){ |
| kenken0721 | 0:158455b60200 | 87 | position = 2350; |
| kenken0721 | 0:158455b60200 | 88 | } |
| kenken0721 | 0:158455b60200 | 89 | |
| kenken0721 | 0:158455b60200 | 90 | RBelbow.SpeedConvert(speed); |
| kenken0721 | 0:158455b60200 | 91 | RBelbow.SetPosition(position); |
| kenken0721 | 0:158455b60200 | 92 | } |
| kenken0721 | 0:158455b60200 | 93 | |
| kenken0721 | 0:158455b60200 | 94 | void LBmove(int position,int speed){ |
| kenken0721 | 0:158455b60200 | 95 | if(2350 <= position){ |
| kenken0721 | 0:158455b60200 | 96 | position = 2350; |
| kenken0721 | 0:158455b60200 | 97 | } |
| kenken0721 | 0:158455b60200 | 98 | |
| kenken0721 | 0:158455b60200 | 99 | LBelbow.SpeedConvert(speed); |
| kenken0721 | 0:158455b60200 | 100 | LBelbow.SetPosition(fmap(position,850,2350,2350,850)); |
| kenken0721 | 0:158455b60200 | 101 | } |
| kenken0721 | 0:158455b60200 | 102 | |
| kenken0721 | 0:158455b60200 | 103 | void forwarding(){ |
| kenken0721 | 0:158455b60200 | 104 | RFmove(1600,3); |
| kenken0721 | 0:158455b60200 | 105 | LFmove(1600,3); |
| kenken0721 | 0:158455b60200 | 106 | RBmove(2200,3); |
| kenken0721 | 0:158455b60200 | 107 | LBmove(2200,3); |
| kenken0721 | 0:158455b60200 | 108 | } |
| kenken0721 | 0:158455b60200 | 109 | |
| kenken0721 | 0:158455b60200 | 110 | float angle = 0; |
| kenken0721 | 0:158455b60200 | 111 | int main() { |
| kenken0721 | 0:158455b60200 | 112 | //初期設定 |
| kenken0721 | 0:158455b60200 | 113 | |
| kenken0721 | 0:158455b60200 | 114 | SerialServo.baud(115200); |
| kenken0721 | 0:158455b60200 | 115 | SerialServo.attach(bluepill_rx, Serial::RxIrq); |
| kenken0721 | 0:158455b60200 | 116 | RFelbow.Enable(fmap(behPos,850,2350,2350,850),330,10); |
| kenken0721 | 0:158455b60200 | 117 | LFelbow.Enable(behPos,330,10); |
| kenken0721 | 0:158455b60200 | 118 | RBelbow.Enable(prePos,330,10); |
| kenken0721 | 0:158455b60200 | 119 | LBelbow.Enable(fmap(prePos,850,2350,2350,850),330,10); |
| kenken0721 | 0:158455b60200 | 120 | |
| kenken0721 | 0:158455b60200 | 121 | RFknee.Enable(downPos,330,50); |
| kenken0721 | 0:158455b60200 | 122 | LFknee.Enable(downPos,330,50); |
| kenken0721 | 0:158455b60200 | 123 | RBknee.Enable(downPos-100,330,50); |
| kenken0721 | 0:158455b60200 | 124 | LBknee.Enable(downPos-100,330,50); |
| kenken0721 | 0:158455b60200 | 125 | |
| kenken0721 | 0:158455b60200 | 126 | serServoMove(0,135,100); |
| kenken0721 | 0:158455b60200 | 127 | serServoMove(1,135,100); |
| kenken0721 | 0:158455b60200 | 128 | serServoMove(2,135,100); |
| kenken0721 | 0:158455b60200 | 129 | serServoMove(3,135,100); |
| kenken0721 | 0:158455b60200 | 130 | forwarding(); |
| kenken0721 | 0:158455b60200 | 131 | wait(5); |
| kenken0721 | 0:158455b60200 | 132 | while (true) { |
| kenken0721 | 0:158455b60200 | 133 | angle = int(20*var); |
| kenken0721 | 0:158455b60200 | 134 | PC.printf("%f \n",angle); |
| kenken0721 | 0:158455b60200 | 135 | centerMove(angle,0); |
| kenken0721 | 0:158455b60200 | 136 | RBknee.SetPosition(downPos -(var*300)); |
| kenken0721 | 0:158455b60200 | 137 | LBknee.SetPosition(downPos -(var*300)); |
| kenken0721 | 0:158455b60200 | 138 | |
| kenken0721 | 0:158455b60200 | 139 | if(Dmode == true){ |
| kenken0721 | 0:158455b60200 | 140 | //RFmove(prePos,50); |
| kenken0721 | 0:158455b60200 | 141 | RFknee.SetPosition(upPos); |
| kenken0721 | 0:158455b60200 | 142 | wait(2.0); |
| kenken0721 | 0:158455b60200 | 143 | RFmove(2350,10); |
| kenken0721 | 0:158455b60200 | 144 | wait(2.0); |
| kenken0721 | 0:158455b60200 | 145 | RFknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 146 | wait(2.0); |
| kenken0721 | 0:158455b60200 | 147 | RFmove(prePos,25); |
| kenken0721 | 0:158455b60200 | 148 | wait(2.0); |
| kenken0721 | 0:158455b60200 | 149 | centerMove(0,0); |
| kenken0721 | 0:158455b60200 | 150 | RBknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 151 | LBknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 152 | |
| kenken0721 | 0:158455b60200 | 153 | RBknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 154 | LBknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 155 | |
| kenken0721 | 0:158455b60200 | 156 | }else{ |
| kenken0721 | 0:158455b60200 | 157 | RFknee.SetPosition(downPos); |
| kenken0721 | 0:158455b60200 | 158 | forwarding(); |
| kenken0721 | 0:158455b60200 | 159 | } |
| kenken0721 | 0:158455b60200 | 160 | |
| kenken0721 | 0:158455b60200 | 161 | wait(0.1); |
| kenken0721 | 0:158455b60200 | 162 | } |
| kenken0721 | 0:158455b60200 | 163 | } |