Pipe team @ Rits / Mbed 2 deprecated Nucleo_AIRo-4_1_anglecontrol

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:bf3a2eb5c7df, 2020-03-22 (annotated)

Committer:
kakogawa
Date:
Sun Mar 22 11:29:35 2020 +0000
Revision:
0:bf3a2eb5c7df
Uploaded by A. Kakogawa, 03.22.2020

Who changed what in which revision?

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