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@1:b2edadf8c3d9, 2018-09-13 (annotated)

Committer:
yuki0701
Date:
Thu Sep 13 13:33:33 2018 +0000
Revision:
1:b2edadf8c3d9
Parent:
0:5cafeb163455
Child:
2:6ac9ac3e5704
a;

Who changed what in which revision?

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