ENPRA_Foot / Mbed 2 deprecated Nucleo_Motor_5_1

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 M3_CAN_mbed

main.cpp@4:5878229a7dfa, 2018-12-25 (annotated)

Committer:
Keith_N
Date:
Tue Dec 25 11:34:30 2018 +0000
Revision:
4:5878229a7dfa
Parent:
3:587916e9f3d0 
changed parameters of a[i]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiroshi1996 0:4134d186d76d 1 #include "mbed.h"
hiroshi1996 1:fca866f76ca0 2 #include "M3_CAN.h"
hiroshi1996 0:4134d186d76d 3
hiroshi1996 0:4134d186d76d 4 //PwmOut led(LED1);
hiroshi1996 0:4134d186d76d 5
hiroshi1996 1:fca866f76ca0 6 M3_CAN_mbed m3(D4, D10, 1000000);
hiroshi1996 1:fca866f76ca0 7
hiroshi1996 0:4134d186d76d 8 Serial Nin(PC_10,PC_11);
hiroshi1996 0:4134d186d76d 9 Serial pc(SERIAL_TX, SERIAL_RX);
hiroshi1996 0:4134d186d76d 10
hiroshi1996 0:4134d186d76d 11 AnalogIn f0(PA_0);
hiroshi1996 0:4134d186d76d 12 AnalogIn f1(PA_1);
hiroshi1996 0:4134d186d76d 13 AnalogIn f2(PA_4);
hiroshi1996 0:4134d186d76d 14 AnalogIn f3(PB_0);
hiroshi1996 0:4134d186d76d 15 AnalogIn f4(PC_1);
hiroshi1996 0:4134d186d76d 16
hiroshi1996 0:4134d186d76d 17 int main()
hiroshi1996 0:4134d186d76d 18 {
hiroshi1996 0:4134d186d76d 19 //float k=0.5;
hiroshi1996 1:fca866f76ca0 20 float data[5];
hiroshi1996 3:587916e9f3d0 21 int32_t a[5]= {0};
hiroshi1996 0:4134d186d76d 22 //uint8_t error;
hiroshi1996 0:4134d186d76d 23 //Nex.baud(9600);
hiroshi1996 0:4134d186d76d 24 //Nex.format(8,Serial::None,2);
hiroshi1996 0:4134d186d76d 25 float force[5];
hiroshi1996 1:fca866f76ca0 26 //float force_r[5];
hiroshi1996 3:587916e9f3d0 27 char tmp[4];
hiroshi1996 0:4134d186d76d 28 //led.period_ms(10);
hiroshi1996 0:4134d186d76d 29
Keith_N 4:5878229a7dfa 30 Nin.baud(115200);
Keith_N 4:5878229a7dfa 31
hiroshi1996 1:fca866f76ca0 32 m3.write(0, RG_SOFT_SRV,RG_DEFAULT, 1);
Keith_N 4:5878229a7dfa 33 m3.write(0, RG_CYCLE,RG_COMMUNICATE, 500000);
Keith_N 4:5878229a7dfa 34 m3.write(0, RG_CYCLE,RG_INTERPOLATE, 100);
Keith_N 4:5878229a7dfa 35 m3.write(0, RG_STATE,RG_DEFAULT, 11);
hiroshi1996 1:fca866f76ca0 36
hiroshi1996 2:34775cb637e6 37 while(1){
hiroshi1996 2:34775cb637e6 38
hiroshi1996 1:fca866f76ca0 39 data[0] = 1-f0;
hiroshi1996 1:fca866f76ca0 40 data[1] = 1-f1;
hiroshi1996 1:fca866f76ca0 41 data[2] = 1-f2;
hiroshi1996 1:fca866f76ca0 42 data[3] = 1-f3;
hiroshi1996 1:fca866f76ca0 43 data[4] = 1-f4;
hiroshi1996 2:34775cb637e6 44
hiroshi1996 2:34775cb637e6 45 for(int j = 0; j <= 4; j++){
hiroshi1996 0:4134d186d76d 46 for(int i = 0;i <= 4; i++){
hiroshi1996 0:4134d186d76d 47 tmp[i] = Nin.getc();
hiroshi1996 0:4134d186d76d 48 }
hiroshi1996 0:4134d186d76d 49 force[j] = atof(tmp);
hiroshi1996 0:4134d186d76d 50 }
hiroshi1996 2:34775cb637e6 51
hiroshi1996 0:4134d186d76d 52 //duty_m = f0;
hiroshi1996 0:4134d186d76d 53 //pc.printf("Duty_m:%.3f\r\n",duty_m);
hiroshi1996 0:4134d186d76d 54 //pc.printf("Duty:%.3f\r\n",duty);
hiroshi1996 0:4134d186d76d 55 //led.write(duty);
hiroshi1996 0:4134d186d76d 56
hiroshi1996 1:fca866f76ca0 57 /* pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
hiroshi1996 2:34775cb637e6 58 ,force[0],force[1],force[2],force[3],force[4]);
hiroshi1996 3:587916e9f3d0 59 pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
hiroshi1996 3:587916e9f3d0 60 ,force[0]-data[0],force[1]-data[1],force[2]-data[2],force[3]-data[2],force[4]-data[2]);
hiroshi1996 3:587916e9f3d0 61
hiroshi1996 1:fca866f76ca0 62 pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
hiroshi1996 3:587916e9f3d0 63 ,data[0],data[1],data[2],data[3],data[4]);
hiroshi1996 3:587916e9f3d0 64 */
hiroshi1996 3:587916e9f3d0 65
hiroshi1996 3:587916e9f3d0 66 // pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
hiroshi1996 3:587916e9f3d0 67 // ,force[0]-data[1],force[1]-data[2],force[2]-data[3],force[3]-data[4]);
hiroshi1996 2:34775cb637e6 68
hiroshi1996 2:34775cb637e6 69 for(int i = 1;i <= 4; i++){
hiroshi1996 3:587916e9f3d0 70 float k = 0.1;
hiroshi1996 1:fca866f76ca0 71 //a+=(int32_t)(30000*diff);
hiroshi1996 3:587916e9f3d0 72 if(force[i]*k-data[i]>=0){
Keith_N 4:5878229a7dfa 73 a[i]+=90000*(force[i]*k-data[i]);
hiroshi1996 3:587916e9f3d0 74 }else{
Keith_N 4:5878229a7dfa 75 a[i]-=810000*(data[i]-force[i]*k);
hiroshi1996 3:587916e9f3d0 76 }
hiroshi1996 1:fca866f76ca0 77
hiroshi1996 1:fca866f76ca0 78 if(a[i]>300000)a[i]=300000;
hiroshi1996 1:fca866f76ca0 79 else if(a[i]<0)a[i]=0;
hiroshi1996 1:fca866f76ca0 80
hiroshi1996 1:fca866f76ca0 81 m3.write(i, RG_REFERENCE,RG_POSITION,a[i]*(-1));
hiroshi1996 3:587916e9f3d0 82
hiroshi1996 2:34775cb637e6 83 pc.printf("No.%d, Stroke:%ld mm ", i, a[i]*2/36000);
hiroshi1996 2:34775cb637e6 84 if(i==4) pc.printf("\n\r");
hiroshi1996 1:fca866f76ca0 85
hiroshi1996 2:34775cb637e6 86 //wait(0.001);
Keith_N 4:5878229a7dfa 87 }
Keith_N 4:5878229a7dfa 88 /* for(int i = 1;i <= 4; i++){
Keith_N 4:5878229a7dfa 89 float k = 0.1;
Keith_N 4:5878229a7dfa 90 //a+=(int32_t)(30000*diff);
Keith_N 4:5878229a7dfa 91 if(force[i]*k-data[i]>=0){
Keith_N 4:5878229a7dfa 92 a[i]+=90000*(0.5f-data[i]);
Keith_N 4:5878229a7dfa 93 }else{
Keith_N 4:5878229a7dfa 94 a[i]-=90000*(data[i]-0.5f);
Keith_N 4:5878229a7dfa 95 }
Keith_N 4:5878229a7dfa 96
Keith_N 4:5878229a7dfa 97 if(a[i]>300000)a[i]=300000;
Keith_N 4:5878229a7dfa 98 else if(a[i]<0)a[i]=0;
Keith_N 4:5878229a7dfa 99
Keith_N 4:5878229a7dfa 100 m3.write(i, RG_REFERENCE,RG_POSITION,a[i]*(-1));
Keith_N 4:5878229a7dfa 101
Keith_N 4:5878229a7dfa 102 pc.printf("No.%d, Stroke:%ld mm ", i, a[i]*2/36000);
Keith_N 4:5878229a7dfa 103 if(i==4) pc.printf("\n\r");
Keith_N 4:5878229a7dfa 104 } */
Keith_N 4:5878229a7dfa 105
hiroshi1996 0:4134d186d76d 106 }
hiroshi1996 0:4134d186d76d 107 }
hiroshi1996 0:4134d186d76d 108
hiroshi1996 1:fca866f76ca0 109