涼太郎 中村 / Servo

a

Dependents:   3servotest 1stcomp 3rdcompfixstart 2ndcomp ... more

Fork of Servo by Tk A

servo.cpp@7:919db47443f4, 2016-09-16 (annotated)

Committer:
choutin
Date:
Fri Sep 16 08:50:07 2016 +0000
Revision:
7:919db47443f4
Parent:
6:b0d581acc298
Child:
8:5a8a6a3b9f33
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:655e1fcb96be 1 #include "mbed.h"
sakanakuuun 0:655e1fcb96be 2
sakanakuuun 0:655e1fcb96be 3 PwmOut pwmarm(PC_6);
sakanakuuun 0:655e1fcb96be 4 PwmOut pwmhand(PC_8);
choutin 1:c8ecfd809e22 5 PwmOut pwmbelt(PC_9);
sakanakuuun 0:655e1fcb96be 6 float PERIOD=20000;
sakanakuuun 0:655e1fcb96be 7
choutin 7:919db47443f4 8 const float btime=1.8;
choutin 7:919db47443f4 9
choutin 1:c8ecfd809e22 10 void armdegree(int degree) {
choutin 1:c8ecfd809e22 11 int i;
choutin 1:c8ecfd809e22 12
choutin 1:c8ecfd809e22 13 pwmarm.period_ms(20); //20ms
choutin 1:c8ecfd809e22 14
choutin 1:c8ecfd809e22 15 degree=10;
choutin 1:c8ecfd809e22 16
choutin 1:c8ecfd809e22 17 i=500+degree*1900/180;
choutin 1:c8ecfd809e22 18 pwmarm.write(i/PERIOD);
choutin 1:c8ecfd809e22 19 }
choutin 1:c8ecfd809e22 20
choutin 1:c8ecfd809e22 21 void handdegree(int degree) {
choutin 1:c8ecfd809e22 22 int i;
choutin 1:c8ecfd809e22 23
choutin 1:c8ecfd809e22 24 pwmarm.period_ms(20); //20ms
choutin 1:c8ecfd809e22 25
choutin 1:c8ecfd809e22 26 degree=10;
choutin 1:c8ecfd809e22 27
choutin 1:c8ecfd809e22 28 i=500+degree*1900/180;
choutin 1:c8ecfd809e22 29 pwmhand.write(i/PERIOD);
choutin 1:c8ecfd809e22 30
choutin 1:c8ecfd809e22 31 }
choutin 1:c8ecfd809e22 32
choutin 3:c112df463a8d 33 void beltup(void){
choutin 1:c8ecfd809e22 34
choutin 1:c8ecfd809e22 35 pwmarm.period_ms(20); //20ms
choutin 3:c112df463a8d 36 pwmbelt.write(0.05);
choutin 1:c8ecfd809e22 37
choutin 1:c8ecfd809e22 38 }
choutin 3:c112df463a8d 39
choutin 3:c112df463a8d 40 void beltdown(void){
choutin 3:c112df463a8d 41
choutin 3:c112df463a8d 42 pwmarm.period_ms(20); //20ms
choutin 3:c112df463a8d 43 pwmbelt.write(0.1);
choutin 3:c112df463a8d 44
choutin 3:c112df463a8d 45 }
choutin 3:c112df463a8d 46
choutin 3:c112df463a8d 47
choutin 4:5ae6ed80dc46 48 void beltstop(void){
choutin 4:5ae6ed80dc46 49
choutin 4:5ae6ed80dc46 50 pwmarm.period_ms(20); //20ms
choutin 4:5ae6ed80dc46 51 pwmbelt.write(0.075);
choutin 4:5ae6ed80dc46 52
choutin 4:5ae6ed80dc46 53 }
choutin 4:5ae6ed80dc46 54
sakanakuuun 0:655e1fcb96be 55 void close_hand(void) {
sakanakuuun 0:655e1fcb96be 56 int i,degree;
sakanakuuun 0:655e1fcb96be 57
sakanakuuun 0:655e1fcb96be 58 pwmhand.period_ms(20); //20ms
sakanakuuun 0:655e1fcb96be 59
choutin 6:b0d581acc298 60 degree=137;
sakanakuuun 0:655e1fcb96be 61
sakanakuuun 0:655e1fcb96be 62 i=500+degree*1900/180;
choutin 3:c112df463a8d 63 pwmhand.write(i/PERIOD);
sakanakuuun 0:655e1fcb96be 64
sakanakuuun 0:655e1fcb96be 65
sakanakuuun 0:655e1fcb96be 66 }
sakanakuuun 0:655e1fcb96be 67
sakanakuuun 0:655e1fcb96be 68 void close_arm(void) {
sakanakuuun 0:655e1fcb96be 69 int i,degree;
sakanakuuun 0:655e1fcb96be 70
sakanakuuun 0:655e1fcb96be 71 pwmarm.period_ms(20); //20ms
sakanakuuun 0:655e1fcb96be 72
choutin 5:58ef29cb8785 73 degree=170;
sakanakuuun 0:655e1fcb96be 74
sakanakuuun 0:655e1fcb96be 75 i=500+degree*1900/180;
sakanakuuun 0:655e1fcb96be 76 pwmarm.write(i/PERIOD);
sakanakuuun 0:655e1fcb96be 77
sakanakuuun 0:655e1fcb96be 78
sakanakuuun 0:655e1fcb96be 79 }
sakanakuuun 0:655e1fcb96be 80
sakanakuuun 0:655e1fcb96be 81
sakanakuuun 0:655e1fcb96be 82 void open_hand(void) {
sakanakuuun 0:655e1fcb96be 83 int i,degree;
sakanakuuun 0:655e1fcb96be 84
sakanakuuun 0:655e1fcb96be 85 pwmhand.period_ms(20); //20ms
sakanakuuun 0:655e1fcb96be 86
choutin 7:919db47443f4 87 degree=83;
sakanakuuun 0:655e1fcb96be 88
sakanakuuun 0:655e1fcb96be 89 i=500+degree*1900/180;
sakanakuuun 0:655e1fcb96be 90 pwmhand.write(i/PERIOD);
sakanakuuun 0:655e1fcb96be 91
sakanakuuun 0:655e1fcb96be 92
sakanakuuun 0:655e1fcb96be 93 }
sakanakuuun 0:655e1fcb96be 94
sakanakuuun 0:655e1fcb96be 95
sakanakuuun 0:655e1fcb96be 96
sakanakuuun 0:655e1fcb96be 97 void open_arm(void) {
sakanakuuun 0:655e1fcb96be 98 int i,degree;
sakanakuuun 0:655e1fcb96be 99
sakanakuuun 0:655e1fcb96be 100 pwmarm.period_ms(20); //20ms
sakanakuuun 0:655e1fcb96be 101
choutin 7:919db47443f4 102 degree=30;
sakanakuuun 0:655e1fcb96be 103
sakanakuuun 0:655e1fcb96be 104 i=500+degree*1900/180;
sakanakuuun 0:655e1fcb96be 105 pwmarm.write(i/PERIOD);
sakanakuuun 0:655e1fcb96be 106
sakanakuuun 0:655e1fcb96be 107
sakanakuuun 0:655e1fcb96be 108 }
choutin 7:919db47443f4 109
choutin 7:919db47443f4 110 void lift(){
choutin 7:919db47443f4 111
choutin 7:919db47443f4 112 beltup();
choutin 7:919db47443f4 113 wait(btime);
choutin 7:919db47443f4 114 beltstop();
choutin 7:919db47443f4 115
choutin 7:919db47443f4 116 }