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@5:2908292a8cf3, 2015-08-12 (annotated)

Committer:
jofo
Date:
Wed Aug 12 14:58:59 2015 +0000
Revision:
5:2908292a8cf3
Parent:
4:3ab1e94b3bc4
Child:
6:3d15e8b4d035
hej august 2015

Who changed what in which revision?

UserRevisionLine numberNew contents of line
samux 2:1db77338562f 1 #include "mbed.h"
samux 2:1db77338562f 2 #include "USBHID.h"
jofo 4:3ab1e94b3bc4 3 #include "WoodenDevice.h"
jofo 4:3ab1e94b3bc4 4
samux 2:1db77338562f 5 //We declare a USBHID device. Input out output reports have a length of 8 bytes
jofo 5:2908292a8cf3 6 USBHID hid(8, 8);
samux 2:1db77338562f 7
samux 2:1db77338562f 8 //This report will contain data to be sent
samux 2:1db77338562f 9 HID_REPORT send_report;
samux 2:1db77338562f 10 HID_REPORT recv_report;
jofo 4:3ab1e94b3bc4 11
jofo 4:3ab1e94b3bc4 12 DigitalOut myled1(LED1);
jofo 4:3ab1e94b3bc4 13 DigitalOut myled2(LED2);
jofo 4:3ab1e94b3bc4 14 DigitalOut myled3(LED3);
jofo 4:3ab1e94b3bc4 15 DigitalOut myled4(LED4);
jofo 4:3ab1e94b3bc4 16
jofo 4:3ab1e94b3bc4 17 DigitalOut enableEscons(p14,1);
samux 2:1db77338562f 18
jofo 4:3ab1e94b3bc4 19 InterruptIn encoder0_A(p5);
jofo 4:3ab1e94b3bc4 20 InterruptIn encoder0_B(p7);
jofo 4:3ab1e94b3bc4 21 InterruptIn encoder1_A(p8);
jofo 4:3ab1e94b3bc4 22 InterruptIn encoder1_B(p10);
jofo 4:3ab1e94b3bc4 23 InterruptIn encoder2_A(p11);
jofo 4:3ab1e94b3bc4 24 InterruptIn encoder2_B(p13);
jofo 4:3ab1e94b3bc4 25
jofo 4:3ab1e94b3bc4 26
jofo 4:3ab1e94b3bc4 27 Serial pc(USBTX, USBRX); // tx, rx
jofo 4:3ab1e94b3bc4 28
jofo 4:3ab1e94b3bc4 29
jofo 4:3ab1e94b3bc4 30 int no_enc = 3;
jofo 4:3ab1e94b3bc4 31
jofo 4:3ab1e94b3bc4 32 int prev_state[3] = {-1,-1,-1};
jofo 4:3ab1e94b3bc4 33 bool encoder_raw[3][2] = {{false,false},{false,false},{false,false}};
jofo 4:3ab1e94b3bc4 34
jofo 4:3ab1e94b3bc4 35 PwmOut pwm[3]={p21,p22,p23};
jofo 4:3ab1e94b3bc4 36 DigitalOut direction[3]={p24,p25,p26};
jofo 4:3ab1e94b3bc4 37
jofo 4:3ab1e94b3bc4 38 int counter[3] = {0,0,0};
jofo 4:3ab1e94b3bc4 39
jofo 4:3ab1e94b3bc4 40 // Pre Cur Dir Dec
jofo 4:3ab1e94b3bc4 41 // 0 0 0 1 + 1
jofo 4:3ab1e94b3bc4 42 // 0 0 1 0 - 2
jofo 4:3ab1e94b3bc4 43 // 0 1 1 1 + 7
jofo 4:3ab1e94b3bc4 44 // 0 1 0 0 - 4
jofo 4:3ab1e94b3bc4 45 // 1 1 1 0 + 14
jofo 4:3ab1e94b3bc4 46 // 1 1 0 1 - 13
jofo 4:3ab1e94b3bc4 47 // 1 0 0 0 + 8
jofo 4:3ab1e94b3bc4 48 // 1 0 1 1 - 11
jofo 4:3ab1e94b3bc4 49 //
jofo 4:3ab1e94b3bc4 50 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
jofo 4:3ab1e94b3bc4 51 //int stable[16] = {0,1,-1,0,-1,0,0,1,1,0, 0,-1, 0,-1, 1, 0}; //OLD
jofo 4:3ab1e94b3bc4 52 int stable[16] = {0,-1,1,0,1,0,0,-1,-1,0, 0,1, 0,1, -1, 0};
jofo 4:3ab1e94b3bc4 53
jofo 4:3ab1e94b3bc4 54 void encoder_callback(int _encoder,int AB,bool value){
jofo 4:3ab1e94b3bc4 55 int cur_state;
jofo 4:3ab1e94b3bc4 56
jofo 4:3ab1e94b3bc4 57 encoder_raw[_encoder][AB]=value;
jofo 4:3ab1e94b3bc4 58
jofo 4:3ab1e94b3bc4 59 cur_state = encoder_raw[_encoder][0] << 1 | encoder_raw[_encoder][1];
jofo 4:3ab1e94b3bc4 60
jofo 4:3ab1e94b3bc4 61 if(prev_state[_encoder] < 0) prev_state[_encoder] = cur_state;
jofo 4:3ab1e94b3bc4 62 counter[_encoder] += stable[prev_state[_encoder] << 2 | cur_state];
jofo 4:3ab1e94b3bc4 63 prev_state[_encoder]=cur_state;
jofo 4:3ab1e94b3bc4 64 }
jofo 4:3ab1e94b3bc4 65
jofo 4:3ab1e94b3bc4 66 // "callback stubs"
jofo 4:3ab1e94b3bc4 67 void callback_0_A_rise(void) { encoder_callback(0,0,true);}
jofo 4:3ab1e94b3bc4 68 void callback_0_A_fall(void) { encoder_callback(0,0,false);}
jofo 4:3ab1e94b3bc4 69 void callback_0_B_rise(void) { encoder_callback(0,1,true);}
jofo 4:3ab1e94b3bc4 70 void callback_0_B_fall(void) { encoder_callback(0,1,false);}
jofo 4:3ab1e94b3bc4 71
jofo 4:3ab1e94b3bc4 72 void callback_1_A_rise(void) { encoder_callback(1,0,true);}
jofo 4:3ab1e94b3bc4 73 void callback_1_A_fall(void) { encoder_callback(1,0,false);}
jofo 4:3ab1e94b3bc4 74 void callback_1_B_rise(void) { encoder_callback(1,1,true);}
jofo 4:3ab1e94b3bc4 75 void callback_1_B_fall(void) { encoder_callback(1,1,false);}
jofo 4:3ab1e94b3bc4 76
jofo 4:3ab1e94b3bc4 77 void callback_2_A_rise(void) { encoder_callback(2,0,true);}
jofo 4:3ab1e94b3bc4 78 void callback_2_A_fall(void) { encoder_callback(2,0,false);}
jofo 4:3ab1e94b3bc4 79 void callback_2_B_rise(void) { encoder_callback(2,1,true);}
jofo 4:3ab1e94b3bc4 80 void callback_2_B_fall(void) { encoder_callback(2,1,false);}
jofo 4:3ab1e94b3bc4 81
jofo 4:3ab1e94b3bc4 82 // Our 12*4=48 byte message (used both up and down)
jofo 4:3ab1e94b3bc4 83 struct woodenhaptics_message {
jofo 4:3ab1e94b3bc4 84 float position_x;
jofo 4:3ab1e94b3bc4 85 float position_y;
jofo 4:3ab1e94b3bc4 86 float position_z;
jofo 4:3ab1e94b3bc4 87 float command_force_x;
jofo 4:3ab1e94b3bc4 88 float command_force_y;
jofo 4:3ab1e94b3bc4 89 float command_force_z;
jofo 4:3ab1e94b3bc4 90 float actual_current_0;
jofo 4:3ab1e94b3bc4 91 float actual_current_1;
jofo 4:3ab1e94b3bc4 92 float actual_current_2;
jofo 4:3ab1e94b3bc4 93 float temperature_0;
jofo 4:3ab1e94b3bc4 94 float temperature_1;
jofo 4:3ab1e94b3bc4 95 float temperature_2;
jofo 4:3ab1e94b3bc4 96
jofo 4:3ab1e94b3bc4 97 woodenhaptics_message():position_x(0),position_y(0),position_z(0),
jofo 4:3ab1e94b3bc4 98 command_force_x(0),command_force_y(0),command_force_z(0),
jofo 4:3ab1e94b3bc4 99 actual_current_0(0),actual_current_1(0),actual_current_2(0),
jofo 4:3ab1e94b3bc4 100 temperature_0(0),temperature_1(0),temperature_2(0){}
jofo 4:3ab1e94b3bc4 101 };
jofo 5:2908292a8cf3 102
jofo 5:2908292a8cf3 103 struct hid_to_pc_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 104 short encoder_a;
jofo 5:2908292a8cf3 105 short encoder_b;
jofo 5:2908292a8cf3 106 short encoder_c;
jofo 5:2908292a8cf3 107 unsigned short debug;
jofo 5:2908292a8cf3 108 };
jofo 5:2908292a8cf3 109
jofo 5:2908292a8cf3 110 struct pc_to_hid_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 111 short current_motor_a_mA;
jofo 5:2908292a8cf3 112 short current_motor_b_mA;
jofo 5:2908292a8cf3 113 short current_motor_c_mA;
jofo 5:2908292a8cf3 114 unsigned int debug;
jofo 5:2908292a8cf3 115 };
jofo 5:2908292a8cf3 116
samux 2:1db77338562f 117
samux 2:1db77338562f 118 int main(void) {
jofo 4:3ab1e94b3bc4 119 myled1 = 1; // SETUP
jofo 4:3ab1e94b3bc4 120 myled2 = 0;
jofo 4:3ab1e94b3bc4 121 myled3 = 0;
jofo 4:3ab1e94b3bc4 122 myled4 = 0;
jofo 4:3ab1e94b3bc4 123
jofo 4:3ab1e94b3bc4 124 encoder0_A.rise(&callback_0_A_rise);
jofo 4:3ab1e94b3bc4 125 encoder0_A.fall(&callback_0_A_fall);
jofo 4:3ab1e94b3bc4 126 encoder0_B.rise(&callback_0_B_rise);
jofo 4:3ab1e94b3bc4 127 encoder0_B.fall(&callback_0_B_fall);
jofo 4:3ab1e94b3bc4 128
jofo 4:3ab1e94b3bc4 129 encoder1_A.rise(&callback_1_A_rise);
jofo 4:3ab1e94b3bc4 130 encoder1_A.fall(&callback_1_A_fall);
jofo 4:3ab1e94b3bc4 131 encoder1_B.rise(&callback_1_B_rise);
jofo 4:3ab1e94b3bc4 132 encoder1_B.fall(&callback_1_B_fall);
jofo 4:3ab1e94b3bc4 133
jofo 4:3ab1e94b3bc4 134 encoder2_A.rise(&callback_2_A_rise);
jofo 4:3ab1e94b3bc4 135 encoder2_A.fall(&callback_2_A_fall);
jofo 4:3ab1e94b3bc4 136 encoder2_B.rise(&callback_2_B_rise);
jofo 4:3ab1e94b3bc4 137 encoder2_B.fall(&callback_2_B_fall);
jofo 4:3ab1e94b3bc4 138
jofo 4:3ab1e94b3bc4 139 enableEscons = 1;
jofo 4:3ab1e94b3bc4 140
jofo 4:3ab1e94b3bc4 141 configuration config = default_woody();
jofo 4:3ab1e94b3bc4 142
jofo 4:3ab1e94b3bc4 143 for(int i=0;i<3;i++){
jofo 4:3ab1e94b3bc4 144 pwm[i].period_us(1000);
jofo 4:3ab1e94b3bc4 145 pwm[i].write(0);
jofo 4:3ab1e94b3bc4 146 direction[i]=0;
jofo 4:3ab1e94b3bc4 147 }
jofo 4:3ab1e94b3bc4 148
jofo 4:3ab1e94b3bc4 149
jofo 4:3ab1e94b3bc4 150 pc.baud(115200);
jofo 4:3ab1e94b3bc4 151
jofo 5:2908292a8cf3 152 send_report.length = 8;
jofo 5:2908292a8cf3 153 //woodenhaptics_message msg;
jofo 5:2908292a8cf3 154
jofo 5:2908292a8cf3 155 hid_to_pc_message hid_to_pc;
jofo 5:2908292a8cf3 156 hid_to_pc.debug = 0;
jofo 5:2908292a8cf3 157 pc_to_hid_message pc_to_hid;
jofo 4:3ab1e94b3bc4 158
jofo 4:3ab1e94b3bc4 159 Timer t;
jofo 4:3ab1e94b3bc4 160 t.start();
jofo 4:3ab1e94b3bc4 161
jofo 4:3ab1e94b3bc4 162 Timer usb_timer;
jofo 4:3ab1e94b3bc4 163 usb_timer.start();
jofo 4:3ab1e94b3bc4 164
jofo 4:3ab1e94b3bc4 165 Timer debug_t;
jofo 4:3ab1e94b3bc4 166 debug_t.start();
jofo 4:3ab1e94b3bc4 167
jofo 4:3ab1e94b3bc4 168
jofo 4:3ab1e94b3bc4 169 pc.printf("Hello World Debug!\n\r");
jofo 4:3ab1e94b3bc4 170
samux 2:1db77338562f 171 while (1) {
jofo 4:3ab1e94b3bc4 172 myled1 = 1;
jofo 4:3ab1e94b3bc4 173 myled2 = 1;
samux 2:1db77338562f 174
jofo 4:3ab1e94b3bc4 175
jofo 4:3ab1e94b3bc4 176
samux 2:1db77338562f 177 //try to read a msg
samux 2:1db77338562f 178 if(hid.readNB(&recv_report)) {
jofo 4:3ab1e94b3bc4 179 // TODO: Make sure we read the latest message, not the oldest.
jofo 4:3ab1e94b3bc4 180 myled3 = !myled3; // We got data
jofo 4:3ab1e94b3bc4 181
jofo 4:3ab1e94b3bc4 182
jofo 5:2908292a8cf3 183 if(recv_report.length == 8){ // It should always be!
jofo 4:3ab1e94b3bc4 184
jofo 5:2908292a8cf3 185 pc_to_hid = *reinterpret_cast<pc_to_hid_message*>(recv_report.data);
jofo 4:3ab1e94b3bc4 186
jofo 5:2908292a8cf3 187 //if(debug_t.read() > 0.5){
jofo 5:2908292a8cf3 188 // pc.printf("Force: %f\n\r",pc_to_hid.current_motor_a_mA);
jofo 5:2908292a8cf3 189 // debug_t.reset();
jofo 5:2908292a8cf3 190 //}
jofo 4:3ab1e94b3bc4 191
jofo 4:3ab1e94b3bc4 192 //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
jofo 4:3ab1e94b3bc4 193
jofo 5:2908292a8cf3 194 //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
jofo 5:2908292a8cf3 195 float f[3] = {pc_to_hid.current_motor_a_mA*0.001, pc_to_hid.current_motor_b_mA*0.001,pc_to_hid.current_motor_c_mA*0.001};
jofo 4:3ab1e94b3bc4 196 for(int i=0;i<3;i++){
jofo 4:3ab1e94b3bc4 197 int dir = f[i] > 0 ? 1 : 0;
jofo 4:3ab1e94b3bc4 198 direction[i].write(dir);
jofo 4:3ab1e94b3bc4 199 // if(i==1)
jofo 4:3ab1e94b3bc4 200 // pc.printf("Direction: %d (direction %d) \n\r", dir, direction[1].read());
jofo 4:3ab1e94b3bc4 201 float abs_val = std::abs(f[i]);
jofo 5:2908292a8cf3 202 if(f[i] > 3.0)
jofo 4:3ab1e94b3bc4 203 pwm[i].write(0.9);
jofo 4:3ab1e94b3bc4 204 else
jofo 5:2908292a8cf3 205 pwm[i].write(0.8*abs_val/3.0+0.1);
jofo 4:3ab1e94b3bc4 206 }
jofo 4:3ab1e94b3bc4 207
jofo 4:3ab1e94b3bc4 208
jofo 4:3ab1e94b3bc4 209 /*
jofo 4:3ab1e94b3bc4 210 pwm[0].write(0.5);
jofo 4:3ab1e94b3bc4 211 pwm[1].write(0.5);
jofo 4:3ab1e94b3bc4 212 pwm[2].write(0.5);
jofo 4:3ab1e94b3bc4 213 */
samux 2:1db77338562f 214 }
jofo 4:3ab1e94b3bc4 215
jofo 4:3ab1e94b3bc4 216 myled1 = 1;
samux 2:1db77338562f 217 }
samux 2:1db77338562f 218
jofo 4:3ab1e94b3bc4 219
jofo 4:3ab1e94b3bc4 220
jofo 4:3ab1e94b3bc4 221 // "move the haptic device to the right"
jofo 5:2908292a8cf3 222 //vec p = getPosition(config, counter);
jofo 5:2908292a8cf3 223 //msg.position_x = p.x;
jofo 5:2908292a8cf3 224 //msg.position_y = p.y;//0.05*sin(t.read());
jofo 5:2908292a8cf3 225 //msg.position_z = p.z;
jofo 4:3ab1e94b3bc4 226
jofo 5:2908292a8cf3 227 //msg.temperature_0 = counter[0]; // TODO: temperature is used temprarily as a carrier for counter values
jofo 5:2908292a8cf3 228 //msg.temperature_1 = counter[1];
jofo 5:2908292a8cf3 229 //msg.temperature_2 = counter[2];
jofo 5:2908292a8cf3 230 hid_to_pc.encoder_a = counter[0];
jofo 5:2908292a8cf3 231 hid_to_pc.encoder_b = counter[1];
jofo 5:2908292a8cf3 232 hid_to_pc.encoder_c = counter[2];
jofo 5:2908292a8cf3 233
jofo 5:2908292a8cf3 234 if(usb_timer.read() > 0.001) {
jofo 4:3ab1e94b3bc4 235 usb_timer.reset();
jofo 4:3ab1e94b3bc4 236
jofo 5:2908292a8cf3 237 hid_to_pc.debug++;
jofo 5:2908292a8cf3 238 unsigned char* out_buf = reinterpret_cast<unsigned char*>(&hid_to_pc);
jofo 4:3ab1e94b3bc4 239
jofo 4:3ab1e94b3bc4 240 //Fill the report
jofo 4:3ab1e94b3bc4 241 for (int i = 0; i < send_report.length; i++) {
jofo 4:3ab1e94b3bc4 242 send_report.data[i] = out_buf[i];
jofo 4:3ab1e94b3bc4 243 }
jofo 4:3ab1e94b3bc4 244
jofo 4:3ab1e94b3bc4 245 //Send the report
jofo 4:3ab1e94b3bc4 246 hid.send(&send_report);
jofo 4:3ab1e94b3bc4 247 myled4 = !myled4;
jofo 4:3ab1e94b3bc4 248 }
jofo 4:3ab1e94b3bc4 249
jofo 4:3ab1e94b3bc4 250 //wait_us(100); // 0.1ms
samux 2:1db77338562f 251 }
samux 0:53dfbb3eae55 252 }