version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
main.cpp@4:3fd7c53d31c1, 2018-04-23 (annotated)
- Committer:
- protongamer
- Date:
- Mon Apr 23 08:44:10 2018 +0000
- Revision:
- 4:3fd7c53d31c1
- Parent:
- 3:86e21a36eecb
- Child:
- 5:d5a021bbe81b
code v1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LouisReynier | 0:1ab5fdb4fa56 | 1 | #include "mbed.h" |
protongamer | 3:86e21a36eecb | 2 | #include "can_parameters.h" |
protongamer | 3:86e21a36eecb | 3 | /* |
protongamer | 3:86e21a36eecb | 4 | Projet BTS SN - Robot à câbles - Lycée Georges Cabanis |
protongamer | 3:86e21a36eecb | 5 | Enzo Niro - Erwin Sazio |
protongamer | 3:86e21a36eecb | 6 | Transmission par Bus CAN(protocol CANOpen) |
protongamer | 3:86e21a36eecb | 7 | |
protongamer | 3:86e21a36eecb | 8 | */ |
LouisReynier | 0:1ab5fdb4fa56 | 9 | |
protongamer | 1:4d70e593345f | 10 | |
protongamer | 3:86e21a36eecb | 11 | |
protongamer | 3:86e21a36eecb | 12 | |
protongamer | 3:86e21a36eecb | 13 | |
protongamer | 3:86e21a36eecb | 14 | |
protongamer | 3:86e21a36eecb | 15 | DigitalOut MOTOR1_OP(LED1); |
protongamer | 3:86e21a36eecb | 16 | DigitalOut MOTOR2_OP(LED2); |
protongamer | 3:86e21a36eecb | 17 | DigitalOut DEB_MOD_DEG(LED3); |
protongamer | 3:86e21a36eecb | 18 | DigitalOut DEB_MOD_NOR(LED4); |
protongamer | 3:86e21a36eecb | 19 | DigitalOut MOD_DEG(p26); |
protongamer | 3:86e21a36eecb | 20 | DigitalOut MOD_NOR(p25); |
protongamer | 3:86e21a36eecb | 21 | DigitalIn SWH(p5); |
protongamer | 3:86e21a36eecb | 22 | DigitalIn SWV(p6); |
protongamer | 3:86e21a36eecb | 23 | DigitalIn BPO(p7); |
protongamer | 3:86e21a36eecb | 24 | |
protongamer | 4:3fd7c53d31c1 | 25 | |
protongamer | 4:3fd7c53d31c1 | 26 | |
LouisReynier | 0:1ab5fdb4fa56 | 27 | CAN can1(p9, p10,1000000);// on definit pin et debit |
LouisReynier | 0:1ab5fdb4fa56 | 28 | CAN can2(p30, p29, 1000000); |
protongamer | 1:4d70e593345f | 29 | Serial pc(USBTX, USBRX); |
protongamer | 1:4d70e593345f | 30 | //AnalogIn pot_1(p19); // potard pour selection |
protongamer | 1:4d70e593345f | 31 | |
protongamer | 1:4d70e593345f | 32 | |
protongamer | 1:4d70e593345f | 33 | void send(int id, char octet_emis[], char RouD, char longueur ) |
protongamer | 1:4d70e593345f | 34 | { |
protongamer | 1:4d70e593345f | 35 | int emis_ok = 0 ; |
LouisReynier | 0:1ab5fdb4fa56 | 36 | |
protongamer | 4:3fd7c53d31c1 | 37 | MOTOR1_OP = 1; |
protongamer | 4:3fd7c53d31c1 | 38 | MOTOR1_OP = 0; |
protongamer | 4:3fd7c53d31c1 | 39 | |
protongamer | 1:4d70e593345f | 40 | if (RouD == 'D') |
protongamer | 1:4d70e593345f | 41 | { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ; |
protongamer | 3:86e21a36eecb | 42 | //pc.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); |
protongamer | 1:4d70e593345f | 43 | }// c'ets la valeur retournée par la fonction write |
protongamer | 1:4d70e593345f | 44 | else |
protongamer | 1:4d70e593345f | 45 | { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard )); |
protongamer | 3:86e21a36eecb | 46 | //pc.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); |
protongamer | 1:4d70e593345f | 47 | } |
protongamer | 1:4d70e593345f | 48 | //lcd.locate(0,10); |
protongamer | 1:4d70e593345f | 49 | //lcd.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); |
protongamer | 3:86e21a36eecb | 50 | /*if(emis_ok) { |
protongamer | 1:4d70e593345f | 51 | // ici octet emis n'a pas de sens car trame remote ! |
protongamer | 3:86e21a36eecb | 52 | //pc.printf("send \r \n"); |
protongamer | 3:86e21a36eecb | 53 | } */ |
LouisReynier | 0:1ab5fdb4fa56 | 54 | } |
protongamer | 1:4d70e593345f | 55 | |
protongamer | 1:4d70e593345f | 56 | |
protongamer | 1:4d70e593345f | 57 | |
protongamer | 4:3fd7c53d31c1 | 58 | |
protongamer | 4:3fd7c53d31c1 | 59 | uint64_t timer_process_read; //counter to make mbed read actual position |
protongamer | 4:3fd7c53d31c1 | 60 | uint64_t timer_process_ping; //counter to make mbed read actual position |
protongamer | 4:3fd7c53d31c1 | 61 | float dy = 525000;//valeur à la POM |
protongamer | 4:3fd7c53d31c1 | 62 | int64_t d_old, d_new; |
protongamer | 4:3fd7c53d31c1 | 63 | bool first_time = 0; |
LouisReynier | 0:1ab5fdb4fa56 | 64 | |
LouisReynier | 0:1ab5fdb4fa56 | 65 | int main() { |
protongamer | 4:3fd7c53d31c1 | 66 | |
protongamer | 4:3fd7c53d31c1 | 67 | |
protongamer | 3:86e21a36eecb | 68 | CANMessage msg; |
protongamer | 4:3fd7c53d31c1 | 69 | uint8_t ping = 0; //variable pour etat de processus |
protongamer | 4:3fd7c53d31c1 | 70 | uint8_t done = 0; |
protongamer | 3:86e21a36eecb | 71 | char command[8]; //word to send command |
protongamer | 4:3fd7c53d31c1 | 72 | |
protongamer | 4:3fd7c53d31c1 | 73 | |
protongamer | 3:86e21a36eecb | 74 | //define Pullup switch |
protongamer | 3:86e21a36eecb | 75 | SWH.mode(PullUp); |
protongamer | 3:86e21a36eecb | 76 | SWV.mode(PullUp); |
protongamer | 3:86e21a36eecb | 77 | BPO.mode(PullUp); |
protongamer | 3:86e21a36eecb | 78 | pc.printf("starting...\r\n"); |
protongamer | 3:86e21a36eecb | 79 | MOD_DEG = 1; |
protongamer | 3:86e21a36eecb | 80 | wait(0.1); |
protongamer | 3:86e21a36eecb | 81 | MOD_DEG = 0; |
protongamer | 3:86e21a36eecb | 82 | wait(0.1); |
protongamer | 3:86e21a36eecb | 83 | MOD_DEG = 1; |
protongamer | 3:86e21a36eecb | 84 | wait(0.1); |
protongamer | 3:86e21a36eecb | 85 | MOD_DEG = 0; |
protongamer | 3:86e21a36eecb | 86 | wait(0.1); |
protongamer | 3:86e21a36eecb | 87 | MOD_DEG = 1; |
protongamer | 3:86e21a36eecb | 88 | wait(0.1); |
protongamer | 3:86e21a36eecb | 89 | MOD_DEG = 0; |
protongamer | 3:86e21a36eecb | 90 | wait(0.1); |
protongamer | 3:86e21a36eecb | 91 | |
protongamer | 3:86e21a36eecb | 92 | command[0] = DRIVER_R1; |
protongamer | 3:86e21a36eecb | 93 | command[1] = DRIVER_R2; |
protongamer | 3:86e21a36eecb | 94 | |
protongamer | 3:86e21a36eecb | 95 | while(ping == 0){ |
protongamer | 3:86e21a36eecb | 96 | pc.printf("ping...\r\n"); |
protongamer | 3:86e21a36eecb | 97 | send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
protongamer | 3:86e21a36eecb | 98 | MOD_NOR = 1; |
protongamer | 3:86e21a36eecb | 99 | if(can2.read(msg)) { |
protongamer | 4:3fd7c53d31c1 | 100 | for(int i = 0; i < msg.len; i++){ |
protongamer | 4:3fd7c53d31c1 | 101 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 4:3fd7c53d31c1 | 102 | ping = 1; |
protongamer | 4:3fd7c53d31c1 | 103 | }//end of for |
protongamer | 4:3fd7c53d31c1 | 104 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 105 | }//can.read(msg) |
protongamer | 3:86e21a36eecb | 106 | wait(0.1); |
protongamer | 3:86e21a36eecb | 107 | MOD_NOR = 0; |
protongamer | 3:86e21a36eecb | 108 | wait(0.9); |
protongamer | 1:4d70e593345f | 109 | |
protongamer | 3:86e21a36eecb | 110 | } |
protongamer | 4:3fd7c53d31c1 | 111 | MOD_NOR = 1; |
protongamer | 4:3fd7c53d31c1 | 112 | for(int working = 0; working < 50; working++){ |
protongamer | 4:3fd7c53d31c1 | 113 | send(TPDO1_R, command, 'R', 0); //Ask position |
protongamer | 4:3fd7c53d31c1 | 114 | timer_process_read = 0; |
protongamer | 4:3fd7c53d31c1 | 115 | d_new = msg.data[3]; |
protongamer | 4:3fd7c53d31c1 | 116 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 117 | d_new = d_new | msg.data[2]; |
protongamer | 4:3fd7c53d31c1 | 118 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 119 | d_new = d_new | msg.data[1]; |
protongamer | 4:3fd7c53d31c1 | 120 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 121 | d_new = d_new | msg.data[0]; |
protongamer | 4:3fd7c53d31c1 | 122 | pc.printf("%d \r\n",d_new);} |
protongamer | 4:3fd7c53d31c1 | 123 | d_old = d_new; |
protongamer | 4:3fd7c53d31c1 | 124 | |
protongamer | 4:3fd7c53d31c1 | 125 | |
protongamer | 1:4d70e593345f | 126 | |
protongamer | 3:86e21a36eecb | 127 | while(1){ |
protongamer | 4:3fd7c53d31c1 | 128 | |
protongamer | 4:3fd7c53d31c1 | 129 | //timer_process_ping++; |
protongamer | 4:3fd7c53d31c1 | 130 | timer_process_read++; |
protongamer | 4:3fd7c53d31c1 | 131 | pc.printf("%d \t",d_new); |
protongamer | 4:3fd7c53d31c1 | 132 | pc.printf("%d \t",d_old); |
protongamer | 4:3fd7c53d31c1 | 133 | //pc.printf("%d \t",d_new); |
protongamer | 4:3fd7c53d31c1 | 134 | pc.printf("%f \r\n",dy); |
protongamer | 4:3fd7c53d31c1 | 135 | |
protongamer | 4:3fd7c53d31c1 | 136 | dy = 525000 + (d_new*1000)/2500; |
protongamer | 4:3fd7c53d31c1 | 137 | |
protongamer | 4:3fd7c53d31c1 | 138 | |
protongamer | 4:3fd7c53d31c1 | 139 | //wait_ms(1); |
protongamer | 4:3fd7c53d31c1 | 140 | |
protongamer | 4:3fd7c53d31c1 | 141 | /* if(timer_process_ping >= 5000){ |
protongamer | 4:3fd7c53d31c1 | 142 | send(TPDO2_R, command, 'R', 0); //Ask mod of motor |
protongamer | 4:3fd7c53d31c1 | 143 | timer_process_ping = 0; |
protongamer | 4:3fd7c53d31c1 | 144 | if(can2.read(msg)) { |
protongamer | 4:3fd7c53d31c1 | 145 | for(int i = 0; i < msg.len; i++){ |
protongamer | 4:3fd7c53d31c1 | 146 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 4:3fd7c53d31c1 | 147 | }//end of for |
protongamer | 4:3fd7c53d31c1 | 148 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 149 | }//can.read(msg) |
protongamer | 4:3fd7c53d31c1 | 150 | |
protongamer | 4:3fd7c53d31c1 | 151 | if(msg.data[0] != 0x27 || msg.data[1] != 0x56){//if device is not in operational mode |
protongamer | 4:3fd7c53d31c1 | 152 | ping = 0; |
protongamer | 4:3fd7c53d31c1 | 153 | while(ping == 0){ |
protongamer | 4:3fd7c53d31c1 | 154 | pc.printf("ping...\r\n"); |
protongamer | 4:3fd7c53d31c1 | 155 | send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
protongamer | 4:3fd7c53d31c1 | 156 | MOD_NOR = 1; |
protongamer | 4:3fd7c53d31c1 | 157 | if(can2.read(msg)) { |
protongamer | 4:3fd7c53d31c1 | 158 | for(int i = 0; i < msg.len; i++){ |
protongamer | 4:3fd7c53d31c1 | 159 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 4:3fd7c53d31c1 | 160 | ping = 1; |
protongamer | 4:3fd7c53d31c1 | 161 | }//end of for |
protongamer | 4:3fd7c53d31c1 | 162 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 163 | }//can.read(msg) |
protongamer | 4:3fd7c53d31c1 | 164 | wait(0.1); |
protongamer | 4:3fd7c53d31c1 | 165 | MOD_NOR = 0; |
protongamer | 4:3fd7c53d31c1 | 166 | wait(0.9); |
protongamer | 4:3fd7c53d31c1 | 167 | |
protongamer | 4:3fd7c53d31c1 | 168 | } |
protongamer | 4:3fd7c53d31c1 | 169 | |
protongamer | 4:3fd7c53d31c1 | 170 | } |
protongamer | 4:3fd7c53d31c1 | 171 | |
protongamer | 4:3fd7c53d31c1 | 172 | }*/ |
protongamer | 4:3fd7c53d31c1 | 173 | |
protongamer | 4:3fd7c53d31c1 | 174 | if(timer_process_read >= 10){ |
protongamer | 4:3fd7c53d31c1 | 175 | send(TPDO1_R, command, 'R', 0); //Ask position |
protongamer | 4:3fd7c53d31c1 | 176 | timer_process_read = 0; |
protongamer | 4:3fd7c53d31c1 | 177 | d_new = msg.data[3]; |
protongamer | 4:3fd7c53d31c1 | 178 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 179 | d_new = d_new | msg.data[2]; |
protongamer | 4:3fd7c53d31c1 | 180 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 181 | d_new = d_new | msg.data[1]; |
protongamer | 4:3fd7c53d31c1 | 182 | d_new = d_new << 8; |
protongamer | 4:3fd7c53d31c1 | 183 | d_new = d_new | msg.data[0]; |
protongamer | 4:3fd7c53d31c1 | 184 | |
protongamer | 4:3fd7c53d31c1 | 185 | /* if(first_time == 0){ |
protongamer | 4:3fd7c53d31c1 | 186 | d_old = d_new; |
protongamer | 4:3fd7c53d31c1 | 187 | first_time = 1; |
protongamer | 4:3fd7c53d31c1 | 188 | }*/ |
protongamer | 4:3fd7c53d31c1 | 189 | } |
protongamer | 4:3fd7c53d31c1 | 190 | |
protongamer | 4:3fd7c53d31c1 | 191 | |
protongamer | 3:86e21a36eecb | 192 | if(can2.read(msg)) { |
protongamer | 3:86e21a36eecb | 193 | for(int i = 0; i < msg.len; i++){ |
protongamer | 3:86e21a36eecb | 194 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 3:86e21a36eecb | 195 | }//end of for |
protongamer | 3:86e21a36eecb | 196 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 197 | }//can.read(msg) |
protongamer | 4:3fd7c53d31c1 | 198 | |
protongamer | 3:86e21a36eecb | 199 | |
protongamer | 3:86e21a36eecb | 200 | switch(BPO){ |
protongamer | 3:86e21a36eecb | 201 | case 0: |
protongamer | 4:3fd7c53d31c1 | 202 | if(done == 0){ |
protongamer | 4:3fd7c53d31c1 | 203 | timer_process_read = 50; |
protongamer | 4:3fd7c53d31c1 | 204 | //timer_process_ping = 0; |
protongamer | 3:86e21a36eecb | 205 | switch(SWV){ |
protongamer | 3:86e21a36eecb | 206 | case 0: |
protongamer | 3:86e21a36eecb | 207 | command[0] = 0x00; |
protongamer | 4:3fd7c53d31c1 | 208 | command[1] = 0x8F; |
protongamer | 4:3fd7c53d31c1 | 209 | command[2] = 0x00; |
protongamer | 3:86e21a36eecb | 210 | command[3] = 0x00; |
protongamer | 3:86e21a36eecb | 211 | send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode |
protongamer | 3:86e21a36eecb | 212 | break; |
protongamer | 3:86e21a36eecb | 213 | |
protongamer | 3:86e21a36eecb | 214 | case 1: |
protongamer | 3:86e21a36eecb | 215 | command[0] = 0xFF; |
protongamer | 4:3fd7c53d31c1 | 216 | command[1] = 0x70; |
protongamer | 4:3fd7c53d31c1 | 217 | command[2] = 0xFF; |
protongamer | 3:86e21a36eecb | 218 | command[3] = 0xFF; |
protongamer | 3:86e21a36eecb | 219 | send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode |
protongamer | 4:3fd7c53d31c1 | 220 | |
protongamer | 3:86e21a36eecb | 221 | |
protongamer | 3:86e21a36eecb | 222 | break; |
protongamer | 4:3fd7c53d31c1 | 223 | }//end of switch SWV |
protongamer | 4:3fd7c53d31c1 | 224 | done = 1; |
protongamer | 3:86e21a36eecb | 225 | } |
protongamer | 3:86e21a36eecb | 226 | |
protongamer | 3:86e21a36eecb | 227 | break; |
protongamer | 3:86e21a36eecb | 228 | |
protongamer | 3:86e21a36eecb | 229 | case 1: |
protongamer | 4:3fd7c53d31c1 | 230 | if(done == 1){ |
protongamer | 3:86e21a36eecb | 231 | command[0] = 0x00; |
protongamer | 3:86e21a36eecb | 232 | command[1] = 0x00; |
protongamer | 3:86e21a36eecb | 233 | command[2] = 0x00; |
protongamer | 3:86e21a36eecb | 234 | command[3] = 0x00; |
protongamer | 3:86e21a36eecb | 235 | send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode |
protongamer | 4:3fd7c53d31c1 | 236 | done = 0; |
protongamer | 4:3fd7c53d31c1 | 237 | //send(TPDO1_R, command, 'R', 0); //send word to put pluto driver in operational mode |
protongamer | 4:3fd7c53d31c1 | 238 | } |
protongamer | 3:86e21a36eecb | 239 | break; |
protongamer | 3:86e21a36eecb | 240 | |
protongamer | 3:86e21a36eecb | 241 | } |
protongamer | 1:4d70e593345f | 242 | |
protongamer | 1:4d70e593345f | 243 | |
protongamer | 1:4d70e593345f | 244 | }// fin while(1) |
protongamer | 1:4d70e593345f | 245 | |
protongamer | 1:4d70e593345f | 246 | } // fin main |
protongamer | 4:3fd7c53d31c1 | 247 | |
protongamer | 4:3fd7c53d31c1 | 248 | |
protongamer | 4:3fd7c53d31c1 | 249 | |
protongamer | 1:4d70e593345f | 250 | |
protongamer | 3:86e21a36eecb | 251 |