亮汰 今井 / Mbed 2 deprecated haru_arm2

アームのスライド移動 まだ片方のアームしか実装されてないけど コピーすれば終わる。

Dependencies:   mbed EC_speedcontroller_syuuseiban

main.cpp@0:d4deb703d536, 2019-01-16 (annotated)

Committer:
ryota1999
Date:
Wed Jan 16 11:17:34 2019 +0000
Revision:
0:d4deb703d536
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryota1999 0:d4deb703d536 1 #include "mbed.h"
ryota1999 0:d4deb703d536 2 #include "EC.h"
ryota1999 0:d4deb703d536 3 #include "math.h"
ryota1999 0:d4deb703d536 4
ryota1999 0:d4deb703d536 5 PwmOut motor1F(PC_9);
ryota1999 0:d4deb703d536 6 PwmOut motor1B(PC_8);
ryota1999 0:d4deb703d536 7 PwmOut motor2F(PC_7);
ryota1999 0:d4deb703d536 8 PwmOut motor2B(PC_6);
ryota1999 0:d4deb703d536 9 Ec ec1(PA_12, PA_11, NC, 500, 0.05);
ryota1999 0:d4deb703d536 10 Ec ec2(PC_4, PB_14, NC, 500, 0.05);
ryota1999 0:d4deb703d536 11 Ticker ticker;
ryota1999 0:d4deb703d536 12 int tag1 = -100;
ryota1999 0:d4deb703d536 13 int tag2 = 100;
ryota1999 0:d4deb703d536 14 int gap1;
ryota1999 0:d4deb703d536 15 int gap2;
ryota1999 0:d4deb703d536 16 int a=0;
ryota1999 0:d4deb703d536 17 int i=1;
ryota1999 0:d4deb703d536 18
ryota1999 0:d4deb703d536 19 void calomega() {
ryota1999 0:d4deb703d536 20 ec1.CalOmega();
ryota1999 0:d4deb703d536 21 ec2.CalOmega();
ryota1999 0:d4deb703d536 22 a=a+1;
ryota1999 0:d4deb703d536 23 if(a>=30&&i==1){
ryota1999 0:d4deb703d536 24 tag1=-400;
ryota1999 0:d4deb703d536 25 a=0;
ryota1999 0:d4deb703d536 26 i=0;
ryota1999 0:d4deb703d536 27 }
ryota1999 0:d4deb703d536 28 if(a>=30&&i==0){
ryota1999 0:d4deb703d536 29 tag1=-100;
ryota1999 0:d4deb703d536 30 a=0;
ryota1999 0:d4deb703d536 31 i=1;
ryota1999 0:d4deb703d536 32 }}
ryota1999 0:d4deb703d536 33
ryota1999 0:d4deb703d536 34 int main() {
ryota1999 0:d4deb703d536 35 ticker.attach(&calomega, 0.05);
ryota1999 0:d4deb703d536 36 motor1F.period_us(50);
ryota1999 0:d4deb703d536 37 motor1B.period_us(50);
ryota1999 0:d4deb703d536 38 motor2F.period_us(50);
ryota1999 0:d4deb703d536 39 motor2B.period_us(50);
ryota1999 0:d4deb703d536 40 while (1) {
ryota1999 0:d4deb703d536 41 printf("count=%d\r\n", ec1.getCount());
ryota1999 0:d4deb703d536 42 gap1 = tag1 - ec1.getCount();
ryota1999 0:d4deb703d536 43 gap2 = tag2 - ec2.getCount();
ryota1999 0:d4deb703d536 44 if (gap1 > 0) {
ryota1999 0:d4deb703d536 45 if (gap1 > 50) {
ryota1999 0:d4deb703d536 46 motor1F = 0;
ryota1999 0:d4deb703d536 47 motor1B = 0.3;
ryota1999 0:d4deb703d536 48 }
ryota1999 0:d4deb703d536 49 if (gap1 < 50) {
ryota1999 0:d4deb703d536 50 motor1F = 0;
ryota1999 0:d4deb703d536 51 motor1B = gap1 * 0.005;
ryota1999 0:d4deb703d536 52 }
ryota1999 0:d4deb703d536 53 }
ryota1999 0:d4deb703d536 54 if (gap1 < 0) {
ryota1999 0:d4deb703d536 55 if (gap1 < -50) {
ryota1999 0:d4deb703d536 56 motor1F = 0.3;
ryota1999 0:d4deb703d536 57 motor1B = 0;
ryota1999 0:d4deb703d536 58 }
ryota1999 0:d4deb703d536 59 if (gap1 > -50) {
ryota1999 0:d4deb703d536 60 motor1F = -gap1 * 0.005;
ryota1999 0:d4deb703d536 61 motor1B = 0;
ryota1999 0:d4deb703d536 62 }
ryota1999 0:d4deb703d536 63 }
ryota1999 0:d4deb703d536 64 }
ryota1999 0:d4deb703d536 65 }
ryota1999 0:d4deb703d536 66