Sera / Mbed 2 deprecated panto

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

main.cpp@0:d91567547359, 2021-03-05 (annotated)

Committer:
serararai
Date:
Fri Mar 05 08:41:22 2021 +0000
Revision:
0:d91567547359
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
serararai 0:d91567547359 1 // Nucleo_CAN_slave (AIRo-4.1)
serararai 0:d91567547359 2 // Angle control
serararai 0:d91567547359 3 // Created by Atsushi Kakogawa, 2019.09.19
serararai 0:d91567547359 4 // Edited by Yoshimichi Oka, 2020.2.5
serararai 0:d91567547359 5 // Modified by Atsushi Kakogawa, 2020.03.22
serararai 0:d91567547359 6 // Department of Robotics, Ritsumeikan University, Japan
serararai 0:d91567547359 7 #include "mbed.h"
serararai 0:d91567547359 8 #include "CAN.h"
serararai 0:d91567547359 9
serararai 0:d91567547359 10 Ticker control; // timer for control
serararai 0:d91567547359 11 DigitalOut myled(PF_1); // LED for communication check
serararai 0:d91567547359 12 DigitalOut mdir1(PA_9); // D1 Motor ON/OFF (HIGH = ON, Changeable on ESCON Studio)
serararai 0:d91567547359 13 DigitalOut mdir2(PA_10); // D0 Rotational direction (Direction is changeable on ESCON Studio)
serararai 0:d91567547359 14 AnalogOut DA_vlc(PA_6); // A5 for velocity analog input to ESCON ANI1+ (0 to 1.0)
serararai 0:d91567547359 15 AnalogOut DA_crt(PA_5); // A3 for current anaglog input to ESCON ANI2+ (0 to 1.0)
serararai 0:d91567547359 16 AnalogIn potensio1(PA_0);
serararai 0:d91567547359 17 //AnalogIn potensio2(PA_7); // A6 Potentiometer 2 (5V potentiometer)
serararai 0:d91567547359 18 AnalogIn AD_crt(PA_1); // Analog Signal from ESCON ANO1 (motor velocity, can be changed by ESCON) (0 to 1.0)
serararai 0:d91567547359 19 AnalogIn AD_vlc(PA_3); // Analog Signal from ESCON AO2 (motor current, can be changed by ESCON) (0 to 1.0)
serararai 0:d91567547359 20
serararai 0:d91567547359 21 float duty = 0;
serararai 0:d91567547359 22 int id = 7;
serararai 0:d91567547359 23 int flag = 0;
serararai 0:d91567547359 24 float target_ang1 = 0.6, ang, e = 0, pang, de = 0;
serararai 0:d91567547359 25 float kp=0.01, kd = 0.1;
serararai 0:d91567547359 26
serararai 0:d91567547359 27 char tx_data[8];
serararai 0:d91567547359 28 char tx_data1_U, tx_data1_L, tx_data2_U, tx_data2_L, tx_data3_U, tx_data3_L;
serararai 0:d91567547359 29
serararai 0:d91567547359 30 // PD-control timer
serararai 0:d91567547359 31 void controller() {
serararai 0:d91567547359 32 ang = (1-potensio1.read())*30;
serararai 0:d91567547359 33 e = target_ang1 - ang;
serararai 0:d91567547359 34 de = ang - pang;
serararai 0:d91567547359 35 DA_crt = kp*abs(e) - kd*abs(de);
serararai 0:d91567547359 36 if (e > 0) {
serararai 0:d91567547359 37 mdir1 = 1;
serararai 0:d91567547359 38 mdir2 = 0;
serararai 0:d91567547359 39 } else if (e < 0) {
serararai 0:d91567547359 40 mdir1 = 1;
serararai 0:d91567547359 41 mdir2 = 1;
serararai 0:d91567547359 42 } else {
serararai 0:d91567547359 43 mdir1 = 0;
serararai 0:d91567547359 44 mdir2 = 0;
serararai 0:d91567547359 45 }
serararai 0:d91567547359 46 pang = ang;
serararai 0:d91567547359 47 }
serararai 0:d91567547359 48
serararai 0:d91567547359 49 int main() {
serararai 0:d91567547359 50
serararai 0:d91567547359 51 control.attach(&controller, 0.001); // 1 ms
serararai 0:d91567547359 52
serararai 0:d91567547359 53 CAN can(PA_11, PA_12);
serararai 0:d91567547359 54 can.frequency(100000);
serararai 0:d91567547359 55 CANMessage msg;
serararai 0:d91567547359 56
serararai 0:d91567547359 57 while(1) {
serararai 0:d91567547359 58 if(can.read(msg)) {
serararai 0:d91567547359 59 if (msg.data[0] == id) { // ID indentify
serararai 0:d91567547359 60 if (msg.data[1] == 0) { // mode indentify (0: control)
serararai 0:d91567547359 61 target_ang1 = (msg.data[3] << 8) + msg.data[4];
serararai 0:d91567547359 62 } else if (msg.data[1] == 1) { // mode indentify (1: response)
serararai 0:d91567547359 63 int i_data1 = AD_crt.read()*1000;
serararai 0:d91567547359 64 tx_data1_U = (i_data1 >> 8) & 0xff;
serararai 0:d91567547359 65 tx_data1_L = i_data1 & 0xff;
serararai 0:d91567547359 66 int i_data2 = AD_vlc.read()*1000;
serararai 0:d91567547359 67 tx_data2_U = (i_data2 >> 8) & 0xff;
serararai 0:d91567547359 68 tx_data2_L = i_data2 & 0xff;
serararai 0:d91567547359 69 int i_data3 = potensio1.read()*330*100;
serararai 0:d91567547359 70 tx_data3_U = (i_data3 >> 8) & 0xff;
serararai 0:d91567547359 71 tx_data3_L = i_data3 & 0xff;
serararai 0:d91567547359 72 tx_data[0] = id; // ID
serararai 0:d91567547359 73 tx_data[1] = 1; // mode (1: response)
serararai 0:d91567547359 74 tx_data[2] = tx_data1_U; // response value1 upper 8bit
serararai 0:d91567547359 75 tx_data[3] = tx_data1_L; // response value1 lower 8bit
serararai 0:d91567547359 76 tx_data[4] = tx_data2_U; // response value2 upper 8bit
serararai 0:d91567547359 77 tx_data[5] = tx_data2_L; // response value2 lower 8bit
serararai 0:d91567547359 78 tx_data[6] = tx_data3_U; // response value3 upper 8bit
serararai 0:d91567547359 79 tx_data[7] = tx_data3_L; // response value3 lower 8bit
serararai 0:d91567547359 80 can.write(CANMessage(1330, tx_data, 8));
serararai 0:d91567547359 81 }
serararai 0:d91567547359 82 }
serararai 0:d91567547359 83 myled = 1; // LED is ON
serararai 0:d91567547359 84 wait (0.01);
serararai 0:d91567547359 85 } else {
serararai 0:d91567547359 86 myled = 0; // LED is OFF
serararai 0:d91567547359 87 }
serararai 0:d91567547359 88 } // while
serararai 0:d91567547359 89 }