robostep8th / Mbed 2 deprecated F3RC-Nucleo-new

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed CruizCore_R1370P

main.cpp@0:5cafeb163455, 2018-09-07 (annotated)

Committer:
yuki0701
Date:
Fri Sep 07 05:19:29 2018 +0000
Revision:
0:5cafeb163455
Child:
1:b2edadf8c3d9
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:5cafeb163455 1 #include "mbed.h"
yuki0701 0:5cafeb163455 2 #include "R1370P.h"
yuki0701 0:5cafeb163455 3
yuki0701 0:5cafeb163455 4 Serial pc(USBTX, USBRX);
yuki0701 0:5cafeb163455 5 R1370P gyro(PA_9,PA_10);
yuki0701 0:5cafeb163455 6
yuki0701 0:5cafeb163455 7 SPISlave spi(PA_7,PA_6,PA_5,PA_4);
yuki0701 0:5cafeb163455 8
yuki0701 0:5cafeb163455 9 PwmOut servo1(PB_4); //servo1 = アサリつかみ（手前）
yuki0701 0:5cafeb163455 10 PwmOut servo2(PB_5); //servo2 = アサリつかみ（奥）
yuki0701 0:5cafeb163455 11 PwmOut servo3(PA_11); //servo3 = アサリアーム
yuki0701 0:5cafeb163455 12 PwmOut servo4(PA_8); //servo4 = ガチアサリアーム
yuki0701 0:5cafeb163455 13
yuki0701 0:5cafeb163455 14 DigitalOut out(PA_1); //エアシリンダー
yuki0701 0:5cafeb163455 15
yuki0701 0:5cafeb163455 16 int main()
yuki0701 0:5cafeb163455 17 {
yuki0701 0:5cafeb163455 18
yuki0701 0:5cafeb163455 19 int flag1=0, flag2=0, flag3=0, flag4=0, flag5=0;
yuki0701 0:5cafeb163455 20 int data1;
yuki0701 0:5cafeb163455 21 int old_data;
yuki0701 0:5cafeb163455 22
yuki0701 0:5cafeb163455 23 servo1.period_ms(20);
yuki0701 0:5cafeb163455 24 servo2.period_ms(20);
yuki0701 0:5cafeb163455 25 servo3.period_ms(20);
yuki0701 0:5cafeb163455 26 servo4.period_ms(20);
yuki0701 0:5cafeb163455 27
yuki0701 0:5cafeb163455 28 spi.format(16,3);
yuki0701 0:5cafeb163455 29 spi.frequency(1000000);
yuki0701 0:5cafeb163455 30
yuki0701 0:5cafeb163455 31 //int debug=0;
yuki0701 0:5cafeb163455 32
yuki0701 0:5cafeb163455 33 double angle, reply_angle;
yuki0701 0:5cafeb163455 34 int a;
yuki0701 0:5cafeb163455 35 gyro.initialize();
yuki0701 0:5cafeb163455 36
yuki0701 0:5cafeb163455 37
yuki0701 0:5cafeb163455 38 while(1) {
yuki0701 0:5cafeb163455 39
yuki0701 0:5cafeb163455 40 //angle = gyro.getAngle();
yuki0701 0:5cafeb163455 41 //printf("%f\r\n",angle);
yuki0701 0:5cafeb163455 42 if(spi.receive()) {
yuki0701 0:5cafeb163455 43 data1=spi.read();
yuki0701 0:5cafeb163455 44
yuki0701 0:5cafeb163455 45 angle = gyro.getAngle();
yuki0701 0:5cafeb163455 46 if(angle >= 0) { //反時計回りに0~359°となるよう修正
yuki0701 0:5cafeb163455 47 a = angle / 360;
yuki0701 0:5cafeb163455 48 reply_angle = (angle - (360 * a))*100; //現在の角度
yuki0701 0:5cafeb163455 49 } else {
yuki0701 0:5cafeb163455 50 a = angle / 360;
yuki0701 0:5cafeb163455 51 reply_angle = (360 + angle - (360 * a))*100;
yuki0701 0:5cafeb163455 52 }
yuki0701 0:5cafeb163455 53
yuki0701 0:5cafeb163455 54 printf("angle = %f\r\n",reply_angle);
yuki0701 0:5cafeb163455 55 spi.reply(reply_angle);
yuki0701 0:5cafeb163455 56
yuki0701 0:5cafeb163455 57 /*printf("%d\r\n",debug);
yuki0701 0:5cafeb163455 58 debug++;
yuki0701 0:5cafeb163455 59 if(debug>100) debug=0;*/
yuki0701 0:5cafeb163455 60 }
yuki0701 0:5cafeb163455 61
yuki0701 0:5cafeb163455 62 if(data1 != old_data) {
yuki0701 0:5cafeb163455 63 if((flag1 == 1) && (data1 == 1)) {
yuki0701 0:5cafeb163455 64 servo1.pulsewidth_us(1000);
yuki0701 0:5cafeb163455 65 flag1=0;
yuki0701 0:5cafeb163455 66 } else if((flag1 == 0) && (data1 == 1)) {
yuki0701 0:5cafeb163455 67 servo1.pulsewidth_us(2050);
yuki0701 0:5cafeb163455 68 flag1=1;
yuki0701 0:5cafeb163455 69 }
yuki0701 0:5cafeb163455 70
yuki0701 0:5cafeb163455 71 if((flag2 == 1) && (data1 == 2)) {
yuki0701 0:5cafeb163455 72 servo2.pulsewidth_us(1000);
yuki0701 0:5cafeb163455 73 flag2=0;
yuki0701 0:5cafeb163455 74 } else if((flag2 == 0) && (data1 == 2)) {
yuki0701 0:5cafeb163455 75 servo2.pulsewidth_us(2050);
yuki0701 0:5cafeb163455 76 flag2=1;
yuki0701 0:5cafeb163455 77 }
yuki0701 0:5cafeb163455 78
yuki0701 0:5cafeb163455 79 if((flag3 == 1) && (data1 == 3)) {
yuki0701 0:5cafeb163455 80 servo3.pulsewidth_us(800);
yuki0701 0:5cafeb163455 81 flag3=0;
yuki0701 0:5cafeb163455 82 } else if((flag3 == 0) && (data1 == 3)) {
yuki0701 0:5cafeb163455 83 servo3.pulsewidth_us(1250);
yuki0701 0:5cafeb163455 84 flag3=1;
yuki0701 0:5cafeb163455 85 }
yuki0701 0:5cafeb163455 86
yuki0701 0:5cafeb163455 87 if((flag4 == 1) && (data1 == 4)) {
yuki0701 0:5cafeb163455 88 servo4.pulsewidth_us(1100);
yuki0701 0:5cafeb163455 89 flag4=0;
yuki0701 0:5cafeb163455 90 } else if((flag4 == 0) && (data1 == 4)) {
yuki0701 0:5cafeb163455 91 servo4.pulsewidth_us(2350);
yuki0701 0:5cafeb163455 92 flag4=1;
yuki0701 0:5cafeb163455 93 }
yuki0701 0:5cafeb163455 94
yuki0701 0:5cafeb163455 95 if((flag5 == 1) && (data1 == 5)) {
yuki0701 0:5cafeb163455 96 out = 1;
yuki0701 0:5cafeb163455 97 flag5=0;
yuki0701 0:5cafeb163455 98 } else if((flag5 == 0) && (data1 == 5)) {
yuki0701 0:5cafeb163455 99 out = 0;
yuki0701 0:5cafeb163455 100 flag5=1;
yuki0701 0:5cafeb163455 101 }
yuki0701 0:5cafeb163455 102 }
yuki0701 0:5cafeb163455 103
yuki0701 0:5cafeb163455 104 old_data = data1;
yuki0701 0:5cafeb163455 105
yuki0701 0:5cafeb163455 106 }
yuki0701 0:5cafeb163455 107 }