Danielle Kruijver / Mbed 2 deprecated Daan_Test1_maxon

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 mbed-rtos

Daan_Test1_maxon.cpp@2:1117ce9d1867, 2019-04-10 (annotated)

Committer:
DanielleKruijver
Date:
Wed Apr 10 09:33:36 2019 +0000
Revision:
2:1117ce9d1867
Parent:
1:822c609fb2ce 
Maxon motoren homen en daarna 300.000qc draaien

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DanielleKruijver 0:785972ab5892 1 #include "mbed.h"
DanielleKruijver 0:785972ab5892 2 #include "Daan_Test1_maxon.h"
DanielleKruijver 0:785972ab5892 3 #include "rtos.h"
DanielleKruijver 0:785972ab5892 4 #define EPOS_DEBUG
DanielleKruijver 0:785972ab5892 5
DanielleKruijver 0:785972ab5892 6 extern RawSerial pc;
DanielleKruijver 0:785972ab5892 7 extern CAN can;
DanielleKruijver 0:785972ab5892 8
DanielleKruijver 2:1117ce9d1867 9 //struct EPOS_MESSAGES EPOS_MSGS;
DanielleKruijver 2:1117ce9d1867 10
DanielleKruijver 2:1117ce9d1867 11 /* -----------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 12 You are going to open the function create_CAN_message. In this function, a
DanielleKruijver 2:1117ce9d1867 13 message is made that consists of the right NODE_ID, Length and the data.
DanielleKruijver 2:1117ce9d1867 14 This data comes from the data that is written in the .h file.
DanielleKruijver 2:1117ce9d1867 15 MessageOut is a CAN message that is part of the funtion create_CAN_msg and
DanielleKruijver 2:1117ce9d1867 16 will be filled with the right data.
DanielleKruijver 2:1117ce9d1867 17 can.write is a function from mbed and gives a 0 or 1 as output:
DanielleKruijver 2:1117ce9d1867 18 0 when nothing is written on the CAN line
DanielleKruijver 2:1117ce9d1867 19 1 when a message is written on the CAN line
DanielleKruijver 2:1117ce9d1867 20 ----------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 21 CANMessage create_CAN_msg(int COB_ID, int LENGTH, char * DATA){
DanielleKruijver 2:1117ce9d1867 22 CANMessage messageOut;
DanielleKruijver 0:785972ab5892 23
DanielleKruijver 2:1117ce9d1867 24 messageOut.format = CANStandard; // standard or extended ID (can be skipped for standard)
DanielleKruijver 0:785972ab5892 25 messageOut.id = COB_ID;
DanielleKruijver 0:785972ab5892 26 messageOut.len = LENGTH;
DanielleKruijver 0:785972ab5892 27
DanielleKruijver 0:785972ab5892 28 for ( int i = 0 ; i<LENGTH ; i++) {
DanielleKruijver 2:1117ce9d1867 29 messageOut.data[i] = DATA[i]; // repeat for each byte.
DanielleKruijver 0:785972ab5892 30 }
DanielleKruijver 0:785972ab5892 31
DanielleKruijver 2:1117ce9d1867 32 if(can.write(messageOut)) { //can.write() gives a 1 or 0 as output
DanielleKruijver 0:785972ab5892 33 #ifdef EPOS_DEBUG
DanielleKruijver 2:1117ce9d1867 34 pc.printf("sending message : "); //output of can.write is 1: print something
DanielleKruijver 0:785972ab5892 35 for ( int ii = 0 ; ii<LENGTH ; ii++) {
DanielleKruijver 2:1117ce9d1867 36 pc.printf("%02x ",messageOut.data[ii]);
DanielleKruijver 0:785972ab5892 37 }
DanielleKruijver 0:785972ab5892 38 pc.printf("\r\n");
DanielleKruijver 0:785972ab5892 39 #endif
DanielleKruijver 0:785972ab5892 40 return 1;
DanielleKruijver 0:785972ab5892 41 }
DanielleKruijver 0:785972ab5892 42 return messageOut;
DanielleKruijver 0:785972ab5892 43 }
DanielleKruijver 2:1117ce9d1867 44
DanielleKruijver 2:1117ce9d1867 45 /* -----------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 46 Make all messages that make use of the funtion CANMessage and return the
DanielleKruijver 2:1117ce9d1867 47 messages.
DanielleKruijver 2:1117ce9d1867 48 ----------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 49 //CANMessage Latest_EPOS_msg(int NODE_ID){
DanielleKruijver 2:1117ce9d1867 50 // return EPOS_MSGS.msg_1;
DanielleKruijver 2:1117ce9d1867 51 // }
DanielleKruijver 0:785972ab5892 52
DanielleKruijver 2:1117ce9d1867 53 CANMessage EPOS::Shutdown(){
DanielleKruijver 0:785972ab5892 54 return create_CAN_msg(0x600+NODE_ID,8,(char*)Shutdown_Data);
DanielleKruijver 0:785972ab5892 55 }
DanielleKruijver 0:785972ab5892 56
DanielleKruijver 2:1117ce9d1867 57 CANMessage EPOS::SwitchOnAndEnable(){
DanielleKruijver 0:785972ab5892 58 return create_CAN_msg(0x600+NODE_ID,8,(char*)Switch_On_And_Enable_Data);
DanielleKruijver 0:785972ab5892 59 }
DanielleKruijver 0:785972ab5892 60
DanielleKruijver 2:1117ce9d1867 61 CANMessage EPOS::StatusWord(){
DanielleKruijver 2:1117ce9d1867 62 return create_CAN_msg(0x600 + NODE_ID,4,(char*)Status_Word_Data);
DanielleKruijver 0:785972ab5892 63 }
DanielleKruijver 0:785972ab5892 64
DanielleKruijver 2:1117ce9d1867 65 CANMessage EPOS::HomingMode(){
DanielleKruijver 0:785972ab5892 66 return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Mode_Data);
DanielleKruijver 0:785972ab5892 67 }
DanielleKruijver 0:785972ab5892 68
DanielleKruijver 2:1117ce9d1867 69 CANMessage EPOS::HomingMethodPositive(){
DanielleKruijver 2:1117ce9d1867 70 return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_Positive);
DanielleKruijver 0:785972ab5892 71 }
DanielleKruijver 0:785972ab5892 72
DanielleKruijver 2:1117ce9d1867 73 CANMessage EPOS::HomingMethodNegative(){
DanielleKruijver 2:1117ce9d1867 74 return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_Negative);
DanielleKruijver 0:785972ab5892 75 }
DanielleKruijver 0:785972ab5892 76
DanielleKruijver 2:1117ce9d1867 77 CANMessage EPOS::StartHoming(){
DanielleKruijver 0:785972ab5892 78 return create_CAN_msg(0x600+NODE_ID,8,(char*)Start_Homing_Data);
DanielleKruijver 0:785972ab5892 79 }
DanielleKruijver 0:785972ab5892 80
DanielleKruijver 2:1117ce9d1867 81 CANMessage EPOS::ClearFault(){
DanielleKruijver 2:1117ce9d1867 82 return create_CAN_msg(0x600+NODE_ID,8,(char*)Clear_Fault_Data);
DanielleKruijver 2:1117ce9d1867 83 }
DanielleKruijver 0:785972ab5892 84
DanielleKruijver 2:1117ce9d1867 85 CANMessage EPOS::PositionMode(){
DanielleKruijver 2:1117ce9d1867 86 return create_CAN_msg(0x600+NODE_ID,8,(char*)Position_Mode_Data);
DanielleKruijver 2:1117ce9d1867 87 }
DanielleKruijver 2:1117ce9d1867 88
DanielleKruijver 2:1117ce9d1867 89 CANMessage EPOS::GoToPosition(int quartercircles){
DanielleKruijver 2:1117ce9d1867 90
DanielleKruijver 2:1117ce9d1867 91 CANMessage msg;
DanielleKruijver 2:1117ce9d1867 92
DanielleKruijver 2:1117ce9d1867 93 msg.format = CANStandard; // standard or extended ID (can be skipped for standard)
DanielleKruijver 2:1117ce9d1867 94 msg.id = 0x600 + NODE_ID;
DanielleKruijver 2:1117ce9d1867 95 msg.len = 8;
DanielleKruijver 0:785972ab5892 96
DanielleKruijver 2:1117ce9d1867 97 /* -------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 98 Unions allow one portion of memory to be accessed as different data types.
DanielleKruijver 2:1117ce9d1867 99 Its declaration and use is similar to the one of structures, but its
DanielleKruijver 2:1117ce9d1867 100 functionality is totally different.
DanielleKruijver 2:1117ce9d1867 101 ------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 102 union {
DanielleKruijver 2:1117ce9d1867 103 int integer;
DanielleKruijver 2:1117ce9d1867 104 unsigned char byte[4];
DanielleKruijver 2:1117ce9d1867 105 } QC;
DanielleKruijver 2:1117ce9d1867 106
DanielleKruijver 2:1117ce9d1867 107 QC.integer = quartercircles;
DanielleKruijver 0:785972ab5892 108
DanielleKruijver 2:1117ce9d1867 109 /* -------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 110 The data that is sent to determine how many quartercircles have to be turned,
DanielleKruijver 2:1117ce9d1867 111 consists of 4 bytes -> byte 0 = 0x23.
DanielleKruijver 2:1117ce9d1867 112 The Position Mode Setting Value has object 0x2062-00(byte1=0x62, byte2=0x20,
DanielleKruijver 2:1117ce9d1867 113 byte3=0x00).
DanielleKruijver 2:1117ce9d1867 114 The data is put in byte 4, 5, 6 and 7 and comes from the union Quaters_Union
DanielleKruijver 2:1117ce9d1867 115 ------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 116 msg.data[0] = 0x23; //0x23 as the first byte: 4 bytes in total sent
DanielleKruijver 2:1117ce9d1867 117 msg.data[1] = 0x62;
DanielleKruijver 2:1117ce9d1867 118 msg.data[2] = 0x20;
DanielleKruijver 2:1117ce9d1867 119 msg.data[3] = 0x00;
DanielleKruijver 2:1117ce9d1867 120 msg.data[4] = QC.byte[0];
DanielleKruijver 2:1117ce9d1867 121 msg.data[5] = QC.byte[1];
DanielleKruijver 2:1117ce9d1867 122 msg.data[6] = QC.byte[2];
DanielleKruijver 2:1117ce9d1867 123 msg.data[7] = QC.byte[3];
DanielleKruijver 2:1117ce9d1867 124
DanielleKruijver 2:1117ce9d1867 125 return msg;
DanielleKruijver 2:1117ce9d1867 126 }
DanielleKruijver 2:1117ce9d1867 127
DanielleKruijver 2:1117ce9d1867 128 /* -----------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 129 With the HOMING function you can home the motor. There are 2 differnt sorts of
DanielleKruijver 2:1117ce9d1867 130 homing:
DanielleKruijver 2:1117ce9d1867 131 - 0xFD: -3: Current treshold positive speed (first in the direction of
DanielleKruijver 2:1117ce9d1867 132 the motor and then to the end to go to the defined position).
DanielleKruijver 2:1117ce9d1867 133 - 0xFC: -4: Current treshold negative speed (this is what we are going
DanielleKruijver 2:1117ce9d1867 134 to use in the real boat: first to the end of then back in the direction
DanielleKruijver 2:1117ce9d1867 135 to the motor).
DanielleKruijver 2:1117ce9d1867 136 For homing different steps have to be executed in the right order. Foo is used
DanielleKruijver 2:1117ce9d1867 137 as a counter to check if all the steps are executed. When can.write is
DanielleKruijver 2:1117ce9d1867 138 succesfull (when there is a message on the CAN line) foo increases with 1. To
DanielleKruijver 2:1117ce9d1867 139 check if all the messages are corrrect, we check it with pc.printf. In tera term
DanielleKruijver 2:1117ce9d1867 140 you do not always get a message if homing is succesfull or failed because the
DanielleKruijver 2:1117ce9d1867 141 4th byte is already 0, but the 5th is 81 so you are not in the situations where
DanielleKruijver 2:1117ce9d1867 142 the output is succes or failed.
DanielleKruijver 2:1117ce9d1867 143
DanielleKruijver 2:1117ce9d1867 144 can.read?
DanielleKruijver 2:1117ce9d1867 145 ----------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 146 void EPOS::Homing(){
DanielleKruijver 2:1117ce9d1867 147
DanielleKruijver 2:1117ce9d1867 148 int foo = 0; //foo is een teller, als can.write lukt komt er een 1 uit en als het faalt een 0. Uiteindelijk moet foo in dit geval dus 5 zijn.
DanielleKruijver 2:1117ce9d1867 149
DanielleKruijver 2:1117ce9d1867 150 foo += can.write(ClearFault());
DanielleKruijver 2:1117ce9d1867 151 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 152 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 2:1117ce9d1867 153
DanielleKruijver 2:1117ce9d1867 154 foo += can.write(HomingMode());
DanielleKruijver 2:1117ce9d1867 155 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 156 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 0:785972ab5892 157
DanielleKruijver 2:1117ce9d1867 158 foo += can.write(HomingMethodPositive());
DanielleKruijver 2:1117ce9d1867 159 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 160 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 0:785972ab5892 161
DanielleKruijver 2:1117ce9d1867 162 foo += can.write(Shutdown());
DanielleKruijver 2:1117ce9d1867 163 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 164 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 2:1117ce9d1867 165
DanielleKruijver 2:1117ce9d1867 166 foo += can.write(SwitchOnAndEnable());
DanielleKruijver 2:1117ce9d1867 167 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 168 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 2:1117ce9d1867 169
DanielleKruijver 2:1117ce9d1867 170 foo += can.write(StartHoming());
DanielleKruijver 2:1117ce9d1867 171 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 172 pc.printf("Waarde van foo is: %i \r\n", foo);
DanielleKruijver 2:1117ce9d1867 173
DanielleKruijver 2:1117ce9d1867 174 CANMessage msg; //msg nog vullen
DanielleKruijver 2:1117ce9d1867 175
DanielleKruijver 2:1117ce9d1867 176 Thread::wait(200);
DanielleKruijver 2:1117ce9d1867 177
DanielleKruijver 2:1117ce9d1867 178 for (int k=0; k<100; k++){ //while(iii<100){
DanielleKruijver 2:1117ce9d1867 179 #ifdef EPOS_DEBUG
DanielleKruijver 2:1117ce9d1867 180 pc.printf("Checking if homing NODE(%i) finished attempt :(%d) \r\n",NODE_ID,k);
DanielleKruijver 2:1117ce9d1867 181 #endif
DanielleKruijver 2:1117ce9d1867 182
DanielleKruijver 2:1117ce9d1867 183 int sw=0;
DanielleKruijver 2:1117ce9d1867 184 sw +=can.write(StatusWord());
DanielleKruijver 2:1117ce9d1867 185 pc.printf("Waarde van sw is: %i \r\n", sw);
DanielleKruijver 2:1117ce9d1867 186
DanielleKruijver 2:1117ce9d1867 187 Thread::wait(100);
DanielleKruijver 2:1117ce9d1867 188
DanielleKruijver 2:1117ce9d1867 189 //msg = Latest_EPOS_msg(NODE_ID); //the NODE_ID is only usefull when you have more messages, then you can use a switch on the basis of the node_id.
DanielleKruijver 2:1117ce9d1867 190
DanielleKruijver 2:1117ce9d1867 191 int teller = 0;
DanielleKruijver 2:1117ce9d1867 192 teller += can.read(msg); //returns 0 if no message, 1 if message
DanielleKruijver 2:1117ce9d1867 193 pc.printf("Waarde van teller is: %i \r\n", teller);
DanielleKruijver 2:1117ce9d1867 194
DanielleKruijver 2:1117ce9d1867 195
DanielleKruijver 2:1117ce9d1867 196 pc.printf("%02x \r\n",msg.data[1]); //printen van de messages die je verstuurd
DanielleKruijver 2:1117ce9d1867 197 pc.printf("%02x \r\n",msg.data[2]); //printen van de messages die je verstuurd
DanielleKruijver 2:1117ce9d1867 198 pc.printf("%02x \r\n",msg.data[4]); //printen van de messages die je verstuurd
DanielleKruijver 2:1117ce9d1867 199 pc.printf("%02x \r\n",msg.data[5]); //printen van de messages die je verstuurd
DanielleKruijver 0:785972ab5892 200
DanielleKruijver 2:1117ce9d1867 201
DanielleKruijver 2:1117ce9d1867 202 if (msg.data[5] == 0x95){ //case home found
DanielleKruijver 2:1117ce9d1867 203 #ifdef EPOS_DEBUG
DanielleKruijver 2:1117ce9d1867 204 pc.printf("Homeing NODE(%i) Succesfull!! \r\n",NODE_ID);
DanielleKruijver 2:1117ce9d1867 205 #endif
DanielleKruijver 2:1117ce9d1867 206 //EPOS_HOME[NODE_ID-1]=1;
DanielleKruijver 2:1117ce9d1867 207 //return; //optional because it is a void?
DanielleKruijver 2:1117ce9d1867 208 }
DanielleKruijver 2:1117ce9d1867 209 else if ( !(msg.data[4] == 0x37)){ //case homing failed
DanielleKruijver 2:1117ce9d1867 210 #ifdef EPOS_DEBUG
DanielleKruijver 2:1117ce9d1867 211 pc.printf("Homeing NODE(%i) FAILED!! \r\n",NODE_ID);
DanielleKruijver 2:1117ce9d1867 212 #endif
DanielleKruijver 2:1117ce9d1867 213 //EPOS_HOME[NODE_ID-1]=0;
DanielleKruijver 2:1117ce9d1867 214 //return;
DanielleKruijver 2:1117ce9d1867 215 }
DanielleKruijver 0:785972ab5892 216
DanielleKruijver 2:1117ce9d1867 217 Thread::wait(100);
DanielleKruijver 2:1117ce9d1867 218 } //end for statement
DanielleKruijver 2:1117ce9d1867 219
DanielleKruijver 2:1117ce9d1867 220 #ifdef EPOS_DEBUG
DanielleKruijver 2:1117ce9d1867 221 printf("\n Homing NODE(%i) timed out \r\n Resetting Device\r\n",NODE_ID);
DanielleKruijver 2:1117ce9d1867 222 #endif
DanielleKruijver 2:1117ce9d1867 223 }
DanielleKruijver 2:1117ce9d1867 224 //can.write(ReSet1());
DanielleKruijver 2:1117ce9d1867 225 //can.write(ReSet2());
DanielleKruijver 2:1117ce9d1867 226 //EPOS_HOME[NODE_ID-1]=0;
DanielleKruijver 2:1117ce9d1867 227
DanielleKruijver 2:1117ce9d1867 228 /* -----------------------------------------------------------------------------
DanielleKruijver 2:1117ce9d1867 229 In this boolean funtion, the position is set and the axis is moving to the new
DanielleKruijver 2:1117ce9d1867 230 absolute position with the maximum acceleration and maximum velocity without a
DanielleKruijver 2:1117ce9d1867 231 particular traject.
DanielleKruijver 2:1117ce9d1867 232 With tel we built in a test to see if all the can messages are written on the
DanielleKruijver 2:1117ce9d1867 233 CAN line.
DanielleKruijver 2:1117ce9d1867 234 ----------------------------------------------------------------------------- */
DanielleKruijver 2:1117ce9d1867 235
DanielleKruijver 2:1117ce9d1867 236 void EPOS::StartPositionMode(){
DanielleKruijver 2:1117ce9d1867 237
DanielleKruijver 2:1117ce9d1867 238 int tel=0;
DanielleKruijver 0:785972ab5892 239
DanielleKruijver 2:1117ce9d1867 240 tel += can.write(ClearFault());
DanielleKruijver 2:1117ce9d1867 241 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 242 pc.printf("Waarde van tel is: %i \r\n", tel);
DanielleKruijver 0:785972ab5892 243
DanielleKruijver 2:1117ce9d1867 244 tel += can.write(PositionMode());
DanielleKruijver 2:1117ce9d1867 245 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 246 pc.printf("Waarde van tel is: %i \r\n", tel);
DanielleKruijver 2:1117ce9d1867 247
DanielleKruijver 2:1117ce9d1867 248 tel += can.write(Shutdown());
DanielleKruijver 2:1117ce9d1867 249 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 250 pc.printf("Waarde van tel is: %i \r\n", tel);
DanielleKruijver 2:1117ce9d1867 251
DanielleKruijver 2:1117ce9d1867 252 tel += can.write(SwitchOnAndEnable());
DanielleKruijver 2:1117ce9d1867 253 Thread::wait(50);
DanielleKruijver 2:1117ce9d1867 254 pc.printf("Waarde van tel is: %i \r\n", tel);
DanielleKruijver 2:1117ce9d1867 255
DanielleKruijver 2:1117ce9d1867 256 float quadc;
DanielleKruijver 2:1117ce9d1867 257 quadc = (int)(-300000); //determine the quadcounts for EPOS
DanielleKruijver 2:1117ce9d1867 258 can.write(GoToPosition(quadc)); //send new position to the controllers
DanielleKruijver 2:1117ce9d1867 259 Thread::wait(100);
DanielleKruijver 0:785972ab5892 260
DanielleKruijver 2:1117ce9d1867 261 }
DanielleKruijver 2:1117ce9d1867 262 //end of StartPositionMode