RaC2018 / Mbed 2 deprecated Robot_a_cables_v3_7

added serial up, down, left and right control

Dependencies:   mbed

Fork of Robot_a_cables_v3_6 by RaC2018

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?

UserRevisionLine numberNew 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