Jonas Forsslund / Mbed 2 deprecated WoodenHapticsHID

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 FastPWM USBDevice

Fork of USBHID_TestCase by Samuel Mokrani

main.cpp@8:ed6b607462de, 2019-05-18 (annotated)

Committer:
jofo
Date:
Sat May 18 11:49:14 2019 +0000
Revision:
8:ed6b607462de
Parent:
7:bb6454b72c57 
Cleaned and tested for local build 2019-05-18

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jofo 8:ed6b607462de 1 /*
jofo 8:ed6b607462de 2 * WoodenHaptics 1.5 USB HID interface
jofo 8:ed6b607462de 3 * Works with the PCB version 0.4, LPC1768 and
jofo 8:ed6b607462de 4 * Escons 50/5 motor controllers using
jofo 8:ed6b607462de 5 * PWM and a direction pin.
jofo 8:ed6b607462de 6 *
jofo 8:ed6b607462de 7 * Developed by Jordi Solsona and Jonas Forsslund
jofo 8:ed6b607462de 8 * (C) Forsslund Systems AB 2015-2019
jofo 8:ed6b607462de 9 * www.woodenhaptics.org
jofo 8:ed6b607462de 10 *
jofo 8:ed6b607462de 11 * Relased as open source under GNU GPL v.3 or later
jofo 8:ed6b607462de 12 * Note: any distribution of binaries (or hardware)
jofo 8:ed6b607462de 13 * Requires this source file, including any modifications,
jofo 8:ed6b607462de 14 * as well as the attribution mentioned above.
jofo 8:ed6b607462de 15 *
jofo 8:ed6b607462de 16 * Updated and tested 2019-05-18 by Jonas Forsslund.
jofo 8:ed6b607462de 17 * jonas@forsslundsystems.com
jofo 8:ed6b607462de 18 *
jofo 8:ed6b607462de 19 */
jofo 8:ed6b607462de 20
samux 2:1db77338562f 21 #include "mbed.h"
samux 2:1db77338562f 22 #include "USBHID.h"
jofo 7:bb6454b72c57 23 #include "FastPWM.h"
jofo 7:bb6454b72c57 24
jofo 4:3ab1e94b3bc4 25
samux 2:1db77338562f 26 //We declare a USBHID device. Input out output reports have a length of 8 bytes
jofo 5:2908292a8cf3 27 USBHID hid(8, 8);
samux 2:1db77338562f 28
samux 2:1db77338562f 29 //This report will contain data to be sent
samux 2:1db77338562f 30 HID_REPORT send_report;
samux 2:1db77338562f 31 HID_REPORT recv_report;
jofo 4:3ab1e94b3bc4 32
jofo 4:3ab1e94b3bc4 33 DigitalOut myled1(LED1);
jofo 4:3ab1e94b3bc4 34 DigitalOut myled2(LED2);
jofo 4:3ab1e94b3bc4 35 DigitalOut myled3(LED3);
jofo 4:3ab1e94b3bc4 36 DigitalOut myled4(LED4);
jofo 4:3ab1e94b3bc4 37
jofo 6:3d15e8b4d035 38 DigitalOut enableEscons(p14,0);
samux 2:1db77338562f 39
jofo 4:3ab1e94b3bc4 40 InterruptIn encoder0_A(p5);
jofo 4:3ab1e94b3bc4 41 InterruptIn encoder0_B(p7);
jofo 4:3ab1e94b3bc4 42 InterruptIn encoder1_A(p8);
jofo 4:3ab1e94b3bc4 43 InterruptIn encoder1_B(p10);
jofo 4:3ab1e94b3bc4 44 InterruptIn encoder2_A(p11);
jofo 4:3ab1e94b3bc4 45 InterruptIn encoder2_B(p13);
jofo 4:3ab1e94b3bc4 46
jofo 6:3d15e8b4d035 47 Timeout msg_watchdog;
jofo 6:3d15e8b4d035 48
jofo 4:3ab1e94b3bc4 49 int no_enc = 3;
jofo 4:3ab1e94b3bc4 50
jofo 4:3ab1e94b3bc4 51 int prev_state[3] = {-1,-1,-1};
jofo 4:3ab1e94b3bc4 52 bool encoder_raw[3][2] = {{false,false},{false,false},{false,false}};
jofo 4:3ab1e94b3bc4 53
jofo 4:3ab1e94b3bc4 54 PwmOut pwm[3]={p21,p22,p23};
jofo 7:bb6454b72c57 55 PwmOut direction[3]={p24,p25,p26};
jofo 4:3ab1e94b3bc4 56
jofo 4:3ab1e94b3bc4 57 int counter[3] = {0,0,0};
jofo 4:3ab1e94b3bc4 58
jofo 4:3ab1e94b3bc4 59 // Pre Cur Dir Dec
jofo 4:3ab1e94b3bc4 60 // 0 0 0 1 + 1
jofo 4:3ab1e94b3bc4 61 // 0 0 1 0 - 2
jofo 4:3ab1e94b3bc4 62 // 0 1 1 1 + 7
jofo 4:3ab1e94b3bc4 63 // 0 1 0 0 - 4
jofo 4:3ab1e94b3bc4 64 // 1 1 1 0 + 14
jofo 4:3ab1e94b3bc4 65 // 1 1 0 1 - 13
jofo 4:3ab1e94b3bc4 66 // 1 0 0 0 + 8
jofo 4:3ab1e94b3bc4 67 // 1 0 1 1 - 11
jofo 4:3ab1e94b3bc4 68 //
jofo 8:ed6b607462de 69 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
jofo 8:ed6b607462de 70 int stable[16] = {0,-1,1,0,1,0,0,-1,-1,0, 0, 1, 0, 1,-1, 0};
jofo 4:3ab1e94b3bc4 71
jofo 4:3ab1e94b3bc4 72 void encoder_callback(int _encoder,int AB,bool value){
jofo 4:3ab1e94b3bc4 73 int cur_state;
jofo 4:3ab1e94b3bc4 74
jofo 4:3ab1e94b3bc4 75 encoder_raw[_encoder][AB]=value;
jofo 4:3ab1e94b3bc4 76
jofo 4:3ab1e94b3bc4 77 cur_state = encoder_raw[_encoder][0] << 1 | encoder_raw[_encoder][1];
jofo 4:3ab1e94b3bc4 78
jofo 4:3ab1e94b3bc4 79 if(prev_state[_encoder] < 0) prev_state[_encoder] = cur_state;
jofo 4:3ab1e94b3bc4 80 counter[_encoder] += stable[prev_state[_encoder] << 2 | cur_state];
jofo 4:3ab1e94b3bc4 81 prev_state[_encoder]=cur_state;
jofo 4:3ab1e94b3bc4 82 }
jofo 4:3ab1e94b3bc4 83
jofo 4:3ab1e94b3bc4 84 // "callback stubs"
jofo 4:3ab1e94b3bc4 85 void callback_0_A_rise(void) { encoder_callback(0,0,true);}
jofo 4:3ab1e94b3bc4 86 void callback_0_A_fall(void) { encoder_callback(0,0,false);}
jofo 4:3ab1e94b3bc4 87 void callback_0_B_rise(void) { encoder_callback(0,1,true);}
jofo 4:3ab1e94b3bc4 88 void callback_0_B_fall(void) { encoder_callback(0,1,false);}
jofo 4:3ab1e94b3bc4 89
jofo 4:3ab1e94b3bc4 90 void callback_1_A_rise(void) { encoder_callback(1,0,true);}
jofo 4:3ab1e94b3bc4 91 void callback_1_A_fall(void) { encoder_callback(1,0,false);}
jofo 4:3ab1e94b3bc4 92 void callback_1_B_rise(void) { encoder_callback(1,1,true);}
jofo 4:3ab1e94b3bc4 93 void callback_1_B_fall(void) { encoder_callback(1,1,false);}
jofo 4:3ab1e94b3bc4 94
jofo 4:3ab1e94b3bc4 95 void callback_2_A_rise(void) { encoder_callback(2,0,true);}
jofo 4:3ab1e94b3bc4 96 void callback_2_A_fall(void) { encoder_callback(2,0,false);}
jofo 4:3ab1e94b3bc4 97 void callback_2_B_rise(void) { encoder_callback(2,1,true);}
jofo 4:3ab1e94b3bc4 98 void callback_2_B_fall(void) { encoder_callback(2,1,false);}
jofo 4:3ab1e94b3bc4 99
jofo 5:2908292a8cf3 100 struct hid_to_pc_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 101 short encoder_a;
jofo 5:2908292a8cf3 102 short encoder_b;
jofo 5:2908292a8cf3 103 short encoder_c;
jofo 5:2908292a8cf3 104 unsigned short debug;
jofo 5:2908292a8cf3 105 };
jofo 5:2908292a8cf3 106
jofo 5:2908292a8cf3 107 struct pc_to_hid_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 108 short current_motor_a_mA;
jofo 5:2908292a8cf3 109 short current_motor_b_mA;
jofo 5:2908292a8cf3 110 short current_motor_c_mA;
jofo 8:ed6b607462de 111 unsigned short debug;
jofo 5:2908292a8cf3 112 };
jofo 5:2908292a8cf3 113
jofo 6:3d15e8b4d035 114 void escon_timeout() {
jofo 6:3d15e8b4d035 115 enableEscons = 0;
jofo 8:ed6b607462de 116 myled2 = 0;
jofo 6:3d15e8b4d035 117 myled3 = 0;
jofo 6:3d15e8b4d035 118 myled4 = 0;
jofo 6:3d15e8b4d035 119
jofo 6:3d15e8b4d035 120 for(int i=0;i<3;i++){
jofo 6:3d15e8b4d035 121 pwm[i].write(0);
jofo 7:bb6454b72c57 122 direction[i].write(0);
jofo 6:3d15e8b4d035 123 }
jofo 6:3d15e8b4d035 124 }
samux 2:1db77338562f 125
samux 2:1db77338562f 126 int main(void) {
jofo 4:3ab1e94b3bc4 127 myled1 = 1; // SETUP
jofo 6:3d15e8b4d035 128 myled2 = 1;
jofo 6:3d15e8b4d035 129 myled3 = 1;
jofo 6:3d15e8b4d035 130 myled4 = 1;
jofo 6:3d15e8b4d035 131
jofo 6:3d15e8b4d035 132 wait_ms(500);
jofo 6:3d15e8b4d035 133 myled1 = 0; // SETUP
jofo 4:3ab1e94b3bc4 134 myled2 = 0;
jofo 4:3ab1e94b3bc4 135 myled3 = 0;
jofo 6:3d15e8b4d035 136 myled4 = 0;
jofo 6:3d15e8b4d035 137
jofo 4:3ab1e94b3bc4 138
jofo 4:3ab1e94b3bc4 139 encoder0_A.rise(&callback_0_A_rise);
jofo 4:3ab1e94b3bc4 140 encoder0_A.fall(&callback_0_A_fall);
jofo 4:3ab1e94b3bc4 141 encoder0_B.rise(&callback_0_B_rise);
jofo 4:3ab1e94b3bc4 142 encoder0_B.fall(&callback_0_B_fall);
jofo 4:3ab1e94b3bc4 143
jofo 4:3ab1e94b3bc4 144 encoder1_A.rise(&callback_1_A_rise);
jofo 4:3ab1e94b3bc4 145 encoder1_A.fall(&callback_1_A_fall);
jofo 4:3ab1e94b3bc4 146 encoder1_B.rise(&callback_1_B_rise);
jofo 4:3ab1e94b3bc4 147 encoder1_B.fall(&callback_1_B_fall);
jofo 4:3ab1e94b3bc4 148
jofo 4:3ab1e94b3bc4 149 encoder2_A.rise(&callback_2_A_rise);
jofo 4:3ab1e94b3bc4 150 encoder2_A.fall(&callback_2_A_fall);
jofo 4:3ab1e94b3bc4 151 encoder2_B.rise(&callback_2_B_rise);
jofo 4:3ab1e94b3bc4 152 encoder2_B.fall(&callback_2_B_fall);
jofo 4:3ab1e94b3bc4 153
jofo 6:3d15e8b4d035 154 enableEscons = 0;
jofo 4:3ab1e94b3bc4 155
jofo 4:3ab1e94b3bc4 156 for(int i=0;i<3;i++){
jofo 7:bb6454b72c57 157 pwm[i].period_us(500);
jofo 4:3ab1e94b3bc4 158 pwm[i].write(0);
jofo 7:bb6454b72c57 159 direction[i].period_us(500);
jofo 7:bb6454b72c57 160 direction[i].write(0);
jofo 8:ed6b607462de 161 }
jofo 8:ed6b607462de 162
jofo 5:2908292a8cf3 163 send_report.length = 8;
jofo 5:2908292a8cf3 164
jofo 5:2908292a8cf3 165 hid_to_pc_message hid_to_pc;
jofo 5:2908292a8cf3 166 hid_to_pc.debug = 0;
jofo 5:2908292a8cf3 167 pc_to_hid_message pc_to_hid;
jofo 8:ed6b607462de 168
jofo 4:3ab1e94b3bc4 169 Timer usb_timer;
jofo 4:3ab1e94b3bc4 170 usb_timer.start();
jofo 4:3ab1e94b3bc4 171
jofo 4:3ab1e94b3bc4 172 Timer debug_t;
jofo 4:3ab1e94b3bc4 173 debug_t.start();
jofo 6:3d15e8b4d035 174
jofo 6:3d15e8b4d035 175 Timer message_timeout;
jofo 6:3d15e8b4d035 176 message_timeout.start();
jofo 6:3d15e8b4d035 177
samux 2:1db77338562f 178 while (1) {
jofo 4:3ab1e94b3bc4 179 myled1 = 1;
jofo 4:3ab1e94b3bc4 180
samux 2:1db77338562f 181 //try to read a msg
samux 2:1db77338562f 182 if(hid.readNB(&recv_report)) {
jofo 8:ed6b607462de 183
jofo 4:3ab1e94b3bc4 184 // TODO: Make sure we read the latest message, not the oldest.
jofo 8:ed6b607462de 185 myled3 = !myled3; // We got data
jofo 4:3ab1e94b3bc4 186
jofo 5:2908292a8cf3 187 if(recv_report.length == 8){ // It should always be!
jofo 8:ed6b607462de 188
jofo 6:3d15e8b4d035 189 msg_watchdog.detach();
jofo 6:3d15e8b4d035 190 msg_watchdog.attach(&escon_timeout,0.5);
jofo 6:3d15e8b4d035 191 enableEscons = 1;
jofo 6:3d15e8b4d035 192 myled2 = 1;
jofo 6:3d15e8b4d035 193
jofo 8:ed6b607462de 194 // Reinterprete the recevied byte array as a pc_to_hid object.
jofo 8:ed6b607462de 195 // Since recv_report.data is defined as unsigned char we
jofo 8:ed6b607462de 196 // have to cast to char* to avoid strict aliasing warning.
jofo 8:ed6b607462de 197 char* dataptr = reinterpret_cast<char*>(recv_report.data);
jofo 8:ed6b607462de 198 pc_to_hid = *reinterpret_cast<pc_to_hid_message*>(dataptr);
jofo 4:3ab1e94b3bc4 199
jofo 8:ed6b607462de 200 float f[3] = {pc_to_hid.current_motor_a_mA*0.001f,
jofo 8:ed6b607462de 201 pc_to_hid.current_motor_b_mA*0.001f,
jofo 8:ed6b607462de 202 pc_to_hid.current_motor_c_mA*0.001f};
jofo 8:ed6b607462de 203
jofo 4:3ab1e94b3bc4 204 for(int i=0;i<3;i++){
jofo 4:3ab1e94b3bc4 205 int dir = f[i] > 0 ? 1 : 0;
jofo 4:3ab1e94b3bc4 206 direction[i].write(dir);
jofo 8:ed6b607462de 207 float abs_val = f[i]<0? -f[i] : f[i];
jofo 7:bb6454b72c57 208 if(abs_val > 3.0)
jofo 4:3ab1e94b3bc4 209 pwm[i].write(0.9);
jofo 4:3ab1e94b3bc4 210 else
jofo 5:2908292a8cf3 211 pwm[i].write(0.8*abs_val/3.0+0.1);
jofo 4:3ab1e94b3bc4 212 }
jofo 8:ed6b607462de 213 }
jofo 4:3ab1e94b3bc4 214 myled1 = 1;
samux 2:1db77338562f 215 }
jofo 7:bb6454b72c57 216
jofo 5:2908292a8cf3 217 hid_to_pc.encoder_a = counter[0];
jofo 5:2908292a8cf3 218 hid_to_pc.encoder_b = counter[1];
jofo 5:2908292a8cf3 219 hid_to_pc.encoder_c = counter[2];
jofo 5:2908292a8cf3 220
jofo 7:bb6454b72c57 221 if(usb_timer.read() > 0.00001) {
jofo 4:3ab1e94b3bc4 222 usb_timer.reset();
jofo 4:3ab1e94b3bc4 223
jofo 5:2908292a8cf3 224 hid_to_pc.debug++;
jofo 5:2908292a8cf3 225 unsigned char* out_buf = reinterpret_cast<unsigned char*>(&hid_to_pc);
jofo 4:3ab1e94b3bc4 226
jofo 4:3ab1e94b3bc4 227 //Fill the report
jofo 8:ed6b607462de 228 for (unsigned int i = 0; i < send_report.length; i++) {
jofo 4:3ab1e94b3bc4 229 send_report.data[i] = out_buf[i];
jofo 4:3ab1e94b3bc4 230 }
jofo 4:3ab1e94b3bc4 231
jofo 4:3ab1e94b3bc4 232 //Send the report
jofo 4:3ab1e94b3bc4 233 hid.send(&send_report);
jofo 4:3ab1e94b3bc4 234 myled4 = !myled4;
jofo 7:bb6454b72c57 235 }
samux 2:1db77338562f 236 }
jofo 8:ed6b607462de 237 }