Danielle Kruijver / Mbed 2 deprecated Controll_SBT_main

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@1:3d46a0660d05, 2019-04-12 (annotated)

Committer:
DanielleKruijver
Date:
Fri Apr 12 09:55:53 2019 +0000
Revision:
1:3d46a0660d05
Main SBT controllll

Who changed what in which revision?

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