Masahiro Furukawa / Mbed 2 deprecated jikken

ticker implemented

Dependencies:   mbed mbed-rtos

jikken.cpp@0:ab5385f7a116, 2020-08-28 (annotated)

Committer:
hiramath
Date:
Fri Aug 28 01:30:06 2020 +0000
Revision:
0:ab5385f7a116
Child:
1:79f80553947c
mbed jikken Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiramath 0:ab5385f7a116 1 #include "mbed.h"
hiramath 0:ab5385f7a116 2 #include <stdio.h>
hiramath 0:ab5385f7a116 3
hiramath 0:ab5385f7a116 4 PwmOut servo0(p21);//θ0 pin
hiramath 0:ab5385f7a116 5 PwmOut servo1(p22);//θ1 pin
hiramath 0:ab5385f7a116 6 PwmOut servo2(p23);//θ2 pin
hiramath 0:ab5385f7a116 7 PwmOut servo3(p24);//θ3 pin
hiramath 0:ab5385f7a116 8 PwmOut servo4(p25);//θ4 pin
hiramath 0:ab5385f7a116 9 PwmOut servo5(p26);//θ5 pin
hiramath 0:ab5385f7a116 10
hiramath 0:ab5385f7a116 11 //for plogram check
hiramath 0:ab5385f7a116 12 DigitalOut led1(LED1);
hiramath 0:ab5385f7a116 13 DigitalOut led2(LED2);
hiramath 0:ab5385f7a116 14 DigitalOut led3(LED3);
hiramath 0:ab5385f7a116 15 DigitalOut led4(LED4);
hiramath 0:ab5385f7a116 16
hiramath 0:ab5385f7a116 17 Serial pc(USBTX, USBRX);
hiramath 0:ab5385f7a116 18 LocalFileSystem local("local");
hiramath 0:ab5385f7a116 19
hiramath 0:ab5385f7a116 20 float aOut, bOut, cOut, dOut, eOut, fOut;//J123456 degree
hiramath 0:ab5385f7a116 21 int pw0, pw1, pw2, pw3, pw4, pw5;//Output Pulse Width
hiramath 0:ab5385f7a116 22
hiramath 0:ab5385f7a116 23 //degree to PulseWidth
hiramath 0:ab5385f7a116 24 int cal_input0(float arg) {
hiramath 0:ab5385f7a116 25 return 1475 + int(10.48 * arg);
hiramath 0:ab5385f7a116 26 }
hiramath 0:ab5385f7a116 27 int cal_input1(float arg) {
hiramath 0:ab5385f7a116 28 return 1218 + int(10.2467 * arg);
hiramath 0:ab5385f7a116 29 }
hiramath 0:ab5385f7a116 30 int cal_input2(float arg) {
hiramath 0:ab5385f7a116 31 return 1306 + 10.59 * arg;
hiramath 0:ab5385f7a116 32 }
hiramath 0:ab5385f7a116 33 int cal_input3(float arg) {
hiramath 0:ab5385f7a116 34 return 1224 + int(8.556 * arg);
hiramath 0:ab5385f7a116 35 }
hiramath 0:ab5385f7a116 36 int cal_input4(float arg) {
hiramath 0:ab5385f7a116 37 return 1460 + int(10.556 * arg);
hiramath 0:ab5385f7a116 38 }
hiramath 0:ab5385f7a116 39 int cal_input5(float arg) {
hiramath 0:ab5385f7a116 40 return 1922 + int(10.556 * arg);
hiramath 0:ab5385f7a116 41 }
hiramath 0:ab5385f7a116 42 //calcurate PulseWidth
hiramath 0:ab5385f7a116 43 void cal_pw()
hiramath 0:ab5385f7a116 44 {
hiramath 0:ab5385f7a116 45 pw0 = cal_input0(aOut);
hiramath 0:ab5385f7a116 46 pw1 = cal_input1(bOut);
hiramath 0:ab5385f7a116 47 pw2 = cal_input2(cOut);
hiramath 0:ab5385f7a116 48 pw3 = cal_input3(dOut);
hiramath 0:ab5385f7a116 49 pw4 = cal_input4(eOut);
hiramath 0:ab5385f7a116 50 pw5 = cal_input5(fOut);
hiramath 0:ab5385f7a116 51 }
hiramath 0:ab5385f7a116 52 //send PulsWidth to servo
hiramath 0:ab5385f7a116 53 void send_servo()
hiramath 0:ab5385f7a116 54 {
hiramath 0:ab5385f7a116 55 servo0.pulsewidth_us(pw0);
hiramath 0:ab5385f7a116 56 servo1.pulsewidth_us(pw1);
hiramath 0:ab5385f7a116 57 servo2.pulsewidth_us(pw2);
hiramath 0:ab5385f7a116 58 servo3.pulsewidth_us(pw3);
hiramath 0:ab5385f7a116 59 servo4.pulsewidth_us(pw4);
hiramath 0:ab5385f7a116 60 servo5.pulsewidth_us(pw5);
hiramath 0:ab5385f7a116 61 }
hiramath 0:ab5385f7a116 62
hiramath 0:ab5385f7a116 63 //move function set
hiramath 0:ab5385f7a116 64 void move()
hiramath 0:ab5385f7a116 65 {
hiramath 0:ab5385f7a116 66 cal_pw();
hiramath 0:ab5385f7a116 67 send_servo();
hiramath 0:ab5385f7a116 68 }
hiramath 0:ab5385f7a116 69 int main(void)
hiramath 0:ab5385f7a116 70 {
hiramath 0:ab5385f7a116 71 //pc.baud(115200);
hiramath 0:ab5385f7a116 72 //pc.format(8, Serial::None, 1);
hiramath 0:ab5385f7a116 73 //pc.printf("start\n");
hiramath 0:ab5385f7a116 74 float w = 60.0;//========Control Frequency===========
hiramath 0:ab5385f7a116 75 float PWMperiod = 1.0 / w;
hiramath 0:ab5385f7a116 76 servo0.period(PWMperiod);
hiramath 0:ab5385f7a116 77 servo1.period(PWMperiod);
hiramath 0:ab5385f7a116 78 servo2.period(PWMperiod);
hiramath 0:ab5385f7a116 79 servo3.period(PWMperiod);
hiramath 0:ab5385f7a116 80 servo4.period(PWMperiod);
hiramath 0:ab5385f7a116 81 servo5.period(PWMperiod);
hiramath 0:ab5385f7a116 82 led4=1.0;
hiramath 0:ab5385f7a116 83 float Out[2548][6];
hiramath 0:ab5385f7a116 84 led2=1.0;
hiramath 0:ab5385f7a116 85 int i;
hiramath 0:ab5385f7a116 86 FILE* fp;
hiramath 0:ab5385f7a116 87
hiramath 0:ab5385f7a116 88 fp = fopen("/local/deg123data_scale_1_4xy.csv", "r");
hiramath 0:ab5385f7a116 89 led1=1.0;
hiramath 0:ab5385f7a116 90 if (fp == NULL)
hiramath 0:ab5385f7a116 91 {
hiramath 0:ab5385f7a116 92 led1 = 1.0;
hiramath 0:ab5385f7a116 93 //pc.printf("error\n");
hiramath 0:ab5385f7a116 94 }
hiramath 0:ab5385f7a116 95 else
hiramath 0:ab5385f7a116 96 {
hiramath 0:ab5385f7a116 97 led3=1.0;
hiramath 0:ab5385f7a116 98 //pc.printf("moving\n");
hiramath 0:ab5385f7a116 99 for(i=0;i<2548;i++)
hiramath 0:ab5385f7a116 100 {
hiramath 0:ab5385f7a116 101 led3=0.0;
hiramath 0:ab5385f7a116 102 fscanf(fp, "%f,%f,%f\n",&Out[i][0],&Out[i][1],&Out[i][2]);
hiramath 0:ab5385f7a116 103 Out[i][3]=0.0;
hiramath 0:ab5385f7a116 104 Out[i][4]=0.0;
hiramath 0:ab5385f7a116 105 Out[i][5]=0.0;
hiramath 0:ab5385f7a116 106 led3=1.0;
hiramath 0:ab5385f7a116 107 aOut = Out[i][0];
hiramath 0:ab5385f7a116 108 bOut = Out[i][1];
hiramath 0:ab5385f7a116 109 cOut = Out[i][2];
hiramath 0:ab5385f7a116 110 dOut = Out[i][3];
hiramath 0:ab5385f7a116 111 eOut = Out[i][4];
hiramath 0:ab5385f7a116 112 fOut = Out[i][5];
hiramath 0:ab5385f7a116 113 move();
hiramath 0:ab5385f7a116 114 wait(0.16);//wait
hiramath 0:ab5385f7a116 115 }
hiramath 0:ab5385f7a116 116 fclose(fp);
hiramath 0:ab5385f7a116 117 led2=1.0;
hiramath 0:ab5385f7a116 118 /*for(i=0;i<2548;i++)
hiramath 0:ab5385f7a116 119 {
hiramath 0:ab5385f7a116 120 aOut = Out[i][0];
hiramath 0:ab5385f7a116 121 bOut = Out[i][1];
hiramath 0:ab5385f7a116 122 cOut = Out[i][2];
hiramath 0:ab5385f7a116 123 dOut = Out[i][3];
hiramath 0:ab5385f7a116 124 eOut = Out[i][4];
hiramath 0:ab5385f7a116 125 fOut = Out[i][5];
hiramath 0:ab5385f7a116 126 move();
hiramath 0:ab5385f7a116 127 //wait_ms(0.16);
hiramath 0:ab5385f7a116 128 } */
hiramath 0:ab5385f7a116 129 }
hiramath 0:ab5385f7a116 130 led1=0.0;
hiramath 0:ab5385f7a116 131 led2=0.0;
hiramath 0:ab5385f7a116 132 led3=0.0;
hiramath 0:ab5385f7a116 133 led4=0.0;
hiramath 0:ab5385f7a116 134 return 0;
hiramath 0:ab5385f7a116 135 }